diff --git a/internal/ceres/array_utils.cc b/internal/ceres/array_utils.cc index 3eea042d5..205ddaf27 100644 --- a/internal/ceres/array_utils.cc +++ b/internal/ceres/array_utils.cc @@ -30,10 +30,11 @@ #include "ceres/array_utils.h" +#include #include #include #include - +#include #include "ceres/fpclassify.h" #include "ceres/stringprintf.h" @@ -94,5 +95,19 @@ void AppendArrayToString(const int size, const double* x, string* result) { } } +void MapValuesToContiguousRange(const int size, int* array) { + std::vector unique_values(array, array + size); + std::sort(unique_values.begin(), unique_values.end()); + unique_values.erase(std::unique(unique_values.begin(), + unique_values.end()), + unique_values.end()); + + for (int i = 0; i < size; ++i) { + array[i] = std::lower_bound(unique_values.begin(), + unique_values.end(), + array[i]) - unique_values.begin(); + } +} + } // namespace internal } // namespace ceres diff --git a/internal/ceres/array_utils.h b/internal/ceres/array_utils.h index 34fda6fd4..55e68a8b5 100644 --- a/internal/ceres/array_utils.h +++ b/internal/ceres/array_utils.h @@ -67,6 +67,14 @@ void AppendArrayToString(const int size, const double* x, string* result); extern const double kImpossibleValue; +// array contains a list of (possibly repeating) non-negative +// integers. Let us assume that we have constructed another array `p` +// by sorting and uniquing the entries of array. +// +// MapValuesToContiguousRange replaces each entry in "array" with its +// position in `p`. +void MapValuesToContiguousRange(int size, int* array); + } // namespace internal } // namespace ceres diff --git a/internal/ceres/array_utils_test.cc b/internal/ceres/array_utils_test.cc index 96e625d62..203a301d3 100644 --- a/internal/ceres/array_utils_test.cc +++ b/internal/ceres/array_utils_test.cc @@ -32,6 +32,7 @@ #include #include +#include #include "gtest/gtest.h" namespace ceres { @@ -71,5 +72,51 @@ TEST(ArrayUtils, FindInvalidIndex) { EXPECT_EQ(FindInvalidValue(3, x), 0); } +TEST(MapValuesToContiguousRange, ContiguousEntries) { + vector array; + array.push_back(0); + array.push_back(1); + vector expected = array; + MapValuesToContiguousRange(array.size(), &array[0]); + EXPECT_EQ(array, expected); + array.clear(); + + array.push_back(1); + array.push_back(0); + expected = array; + MapValuesToContiguousRange(array.size(), &array[0]); + EXPECT_EQ(array, expected); +} + +TEST(MapValuesToContiguousRange, NonContiguousEntries) { + vector array; + array.push_back(0); + array.push_back(2); + vector expected; + expected.push_back(0); + expected.push_back(1); + MapValuesToContiguousRange(array.size(), &array[0]); + EXPECT_EQ(array, expected); +} + +TEST(MapValuesToContiguousRange, NonContiguousRepeatingEntries) { + vector array; + array.push_back(3); + array.push_back(1); + array.push_back(0); + array.push_back(0); + array.push_back(0); + array.push_back(5); + vector expected; + expected.push_back(2); + expected.push_back(1); + expected.push_back(0); + expected.push_back(0); + expected.push_back(0); + expected.push_back(3); + MapValuesToContiguousRange(array.size(), &array[0]); + EXPECT_EQ(array, expected); +} + } // namespace internal } // namespace ceres diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index 2bf6cd262..fbcaf6b03 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc @@ -34,6 +34,7 @@ #include // NOLINT #include #include +#include "ceres/array_utils.h" #include "ceres/coordinate_descent_minimizer.h" #include "ceres/cxsparse.h" #include "ceres/evaluator.h" @@ -1707,7 +1708,7 @@ bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( // Renumber the entries of constraints to be contiguous integers // as camd requires that the group ids be in the range [0, // parameter_blocks.size() - 1]. - SolverImpl::CompactifyArray(&constraints); + MapValuesToContiguousRange(constraints.size(), &constraints[0]); // Set the offsets and index for CreateJacobianSparsityTranspose. program->SetParameterOffsetsAndIndex(); @@ -1832,20 +1833,5 @@ bool SolverImpl::ReorderProgramForSparseNormalCholesky( return true; } -void SolverImpl::CompactifyArray(vector* array_ptr) { - vector& array = *array_ptr; - const set unique_group_ids(array.begin(), array.end()); - map group_id_map; - for (set::const_iterator it = unique_group_ids.begin(); - it != unique_group_ids.end(); - ++it) { - InsertOrDie(&group_id_map, *it, group_id_map.size()); - } - - for (int i = 0; i < array.size(); ++i) { - array[i] = group_id_map[array[i]]; - } -} - } // namespace internal } // namespace ceres diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h index 631add59b..0085898bc 100644 --- a/internal/ceres/solver_impl.h +++ b/internal/ceres/solver_impl.h @@ -213,13 +213,6 @@ class SolverImpl { ParameterBlockOrdering* parameter_block_ordering, Program* program, string* message); - - // array contains a list of (possibly repeating) non-negative - // integers. Let us assume that we have constructed another array - // `p` by sorting and uniqueing the entries of array. - // CompactifyArray replaces each entry in "array" with its position - // in `p`. - static void CompactifyArray(vector* array); }; } // namespace internal diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc index c2f8c2223..1f156286f 100644 --- a/internal/ceres/solver_impl_test.cc +++ b/internal/ceres/solver_impl_test.cc @@ -1038,53 +1038,6 @@ TEST(SolverImpl, ReallocationInCreateJacobianBlockSparsityTranspose) { EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0); } -TEST(CompactifyArray, ContiguousEntries) { - vector array; - array.push_back(0); - array.push_back(1); - vector expected = array; - SolverImpl::CompactifyArray(&array); - EXPECT_EQ(array, expected); - array.clear(); - - array.push_back(1); - array.push_back(0); - expected = array; - SolverImpl::CompactifyArray(&array); - EXPECT_EQ(array, expected); -} - -TEST(CompactifyArray, NonContiguousEntries) { - vector array; - array.push_back(0); - array.push_back(2); - vector expected; - expected.push_back(0); - expected.push_back(1); - SolverImpl::CompactifyArray(&array); - EXPECT_EQ(array, expected); -} - -TEST(CompactifyArray, NonContiguousRepeatingEntries) { - vector array; - array.push_back(3); - array.push_back(1); - array.push_back(0); - array.push_back(0); - array.push_back(0); - array.push_back(5); - vector expected; - expected.push_back(2); - expected.push_back(1); - expected.push_back(0); - expected.push_back(0); - expected.push_back(0); - expected.push_back(3); - - SolverImpl::CompactifyArray(&array); - EXPECT_EQ(array, expected); -} - TEST(SolverImpl, ProblemHasNanParameterBlocks) { Problem problem; double x[2];