Start of the new ordering API. Change-Id: I37b0f39011f590d54962ad3e1da1f42712008f82
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index cd4e6ac..862b7cf 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -63,6 +63,7 @@ local_parameterization.cc loss_function.cc normal_prior.cc + ordering.cc partitioned_matrix_view.cc polynomial_solver.cc problem.cc @@ -232,6 +233,7 @@ CERES_TEST(minimizer) CERES_TEST(normal_prior) CERES_TEST(numeric_diff_cost_function) + CERES_TEST(ordering) CERES_TEST(parameter_block) CERES_TEST(partitioned_matrix_view) CERES_TEST(polynomial_solver)
diff --git a/internal/ceres/ordering.cc b/internal/ceres/ordering.cc new file mode 100644 index 0000000..8b53ea5 --- /dev/null +++ b/internal/ceres/ordering.cc
@@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <map> +#include <set> +#include "ceres/internal/port.h" +#include "ceres/ordering.h" +#include "glog/logging.h" + +namespace ceres { + +void Ordering::AddParameterBlockToGroup(double* parameter_block, int group_id) { + const map<double*, int>::const_iterator it = + parameter_block_to_group_id_.find(parameter_block); + + if (it != parameter_block_to_group_id_.end()) { + group_id_to_parameter_blocks_[it->second].erase(parameter_block); + if (group_id_to_parameter_blocks_[it->second].size() == 0) { + group_id_to_parameter_blocks_.erase(it->second); + } + } + + parameter_block_to_group_id_[parameter_block] = group_id; + group_id_to_parameter_blocks_[group_id].insert(parameter_block); +} + +void Ordering::RemoveParameterBlock(double* parameter_block) { + const int current_group_id = GroupIdForParameterBlock(parameter_block); + group_id_to_parameter_blocks_[current_group_id].erase(parameter_block); + if (group_id_to_parameter_blocks_[current_group_id].size() == 0) { + group_id_to_parameter_blocks_.erase(current_group_id); + } + + parameter_block_to_group_id_.erase(parameter_block); +} + +int Ordering::GroupIdForParameterBlock(double* parameter_block) const { + const map<double*, int>::const_iterator it = + parameter_block_to_group_id_.find(parameter_block); + CHECK(it != parameter_block_to_group_id_.end()) + << "Tried finding the group id of a parameter block unknown to " + << "the ordering: " << parameter_block; + return it->second; +} + +int Ordering::NumParameterBlocks() const { + return parameter_block_to_group_id_.size(); +} + +int Ordering::NumGroups() const { return group_id_to_parameter_blocks_.size(); } + +int Ordering::GroupSize(int group_id) const { + map<int, set<double*> >::const_iterator it = + group_id_to_parameter_blocks_.find(group_id); + return (it == group_id_to_parameter_blocks_.end()) ? 0 : it->second.size(); +} + +const map<int, set<double*> >& Ordering::group_id_to_parameter_blocks() const { + return group_id_to_parameter_blocks_; +} + +} // namespace ceres
diff --git a/internal/ceres/ordering_test.cc b/internal/ceres/ordering_test.cc new file mode 100644 index 0000000..9bd43b3 --- /dev/null +++ b/internal/ceres/ordering_test.cc
@@ -0,0 +1,140 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/ordering.h" + +#include <cstddef> +#include <vector> +#include "gtest/gtest.h" +#include "ceres/collections_port.h" + +namespace ceres { +namespace internal { + +TEST(Ordering, EmptyOrderingBehavesCorrectly) { + Ordering ordering; + EXPECT_EQ(ordering.NumGroups(), 0); + EXPECT_EQ(ordering.NumParameterBlocks(), 0); + EXPECT_EQ(ordering.GroupSize(1), 0); + double x; + EXPECT_DEATH_IF_SUPPORTED(ordering.GroupIdForParameterBlock(&x), + "Tried finding"); + EXPECT_DEATH_IF_SUPPORTED(ordering.RemoveParameterBlock(&x), + "Tried finding"); +} + +TEST(Ordering, EverythingInOneGroup) { + Ordering ordering; + double x[3]; + ordering.AddParameterBlockToGroup(x, 1); + ordering.AddParameterBlockToGroup(x + 1, 1); + ordering.AddParameterBlockToGroup(x + 2, 1); + ordering.AddParameterBlockToGroup(x, 1); + + EXPECT_EQ(ordering.NumGroups(), 1); + EXPECT_EQ(ordering.NumParameterBlocks(), 3); + EXPECT_EQ(ordering.GroupSize(1), 3); + EXPECT_EQ(ordering.GroupSize(0), 0); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 1), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 2), 1); + + ordering.RemoveParameterBlock(x); + EXPECT_EQ(ordering.NumGroups(), 1); + EXPECT_EQ(ordering.NumParameterBlocks(), 2); + EXPECT_EQ(ordering.GroupSize(1), 2); + EXPECT_EQ(ordering.GroupSize(0), 0); + + EXPECT_DEATH_IF_SUPPORTED(ordering.GroupIdForParameterBlock(x), + "Tried finding"); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 1), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 2), 1); +} + +TEST(Ordering, StartInOneGroupAndThenSplit) { + Ordering ordering; + double x[3]; + ordering.AddParameterBlockToGroup(x, 1); + ordering.AddParameterBlockToGroup(x + 1, 1); + ordering.AddParameterBlockToGroup(x + 2, 1); + ordering.AddParameterBlockToGroup(x, 1); + + EXPECT_EQ(ordering.NumGroups(), 1); + EXPECT_EQ(ordering.NumParameterBlocks(), 3); + EXPECT_EQ(ordering.GroupSize(1), 3); + EXPECT_EQ(ordering.GroupSize(0), 0); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 1), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 2), 1); + + ordering.AddParameterBlockToGroup(x, 5); + EXPECT_EQ(ordering.NumGroups(), 2); + EXPECT_EQ(ordering.NumParameterBlocks(), 3); + EXPECT_EQ(ordering.GroupSize(1), 2); + EXPECT_EQ(ordering.GroupSize(5), 1); + EXPECT_EQ(ordering.GroupSize(0), 0); + + EXPECT_EQ(ordering.GroupIdForParameterBlock(x), 5); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 1), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 2), 1); +} + +TEST(Ordering, AddAndRemoveEveryThingFromOneGroup) { + Ordering ordering; + double x[3]; + ordering.AddParameterBlockToGroup(x, 1); + ordering.AddParameterBlockToGroup(x + 1, 1); + ordering.AddParameterBlockToGroup(x + 2, 1); + ordering.AddParameterBlockToGroup(x, 1); + + EXPECT_EQ(ordering.NumGroups(), 1); + EXPECT_EQ(ordering.NumParameterBlocks(), 3); + EXPECT_EQ(ordering.GroupSize(1), 3); + EXPECT_EQ(ordering.GroupSize(0), 0); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 1), 1); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 2), 1); + + ordering.AddParameterBlockToGroup(x, 5); + ordering.AddParameterBlockToGroup(x + 1, 5); + ordering.AddParameterBlockToGroup(x + 2, 5); + EXPECT_EQ(ordering.NumGroups(), 1); + EXPECT_EQ(ordering.NumParameterBlocks(), 3); + EXPECT_EQ(ordering.GroupSize(1), 0); + EXPECT_EQ(ordering.GroupSize(5), 3); + EXPECT_EQ(ordering.GroupSize(0), 0); + + EXPECT_EQ(ordering.GroupIdForParameterBlock(x), 5); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 1), 5); + EXPECT_EQ(ordering.GroupIdForParameterBlock(x + 2), 5); +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index 0ece8ee..8f2f38b 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc
@@ -40,6 +40,7 @@ #include "ceres/linear_solver.h" #include "ceres/map_util.h" #include "ceres/minimizer.h" +#include "ceres/ordering.h" #include "ceres/parameter_block.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" @@ -201,6 +202,51 @@ Solver::Summary* summary) { double solver_start_time = WallTimeInSeconds(); Solver::Options options(original_options); + + // Code for supporting both the old and the new ordering API. This + // code will get refactored and re-written as the old API is + // steadily deprecated. + + // Clobber all the input parameters from the old api parameters. + if (options.use_new_ordering_api) { + // For linear solvers which are not of Schur type, do nothing. + options.ordering.clear(); + options.num_eliminate_blocks = 0; + options.ordering_type = NATURAL; + + if (IsSchurType(options.linear_solver_type)) { + if (options.ordering_new_api == NULL) { + // User says we are free to find the independent set, and order + // any which way. + options.ordering_type = SCHUR; + } else { + // User has given an ordering and asked for a Schur type solver. + options.ordering_type = USER; + + // The lowest numbered group corresponds to + // num_eliminate_blocks e_blocks. + const map<int, set<double*> >& group_id_to_parameter_block + = options.ordering_new_api->group_id_to_parameter_blocks(); + + map<int, set<double*> >::const_iterator it = + group_id_to_parameter_block.begin(); + CHECK(it != group_id_to_parameter_block.end()); + + options.num_eliminate_blocks = it->second.size(); + for (; it != group_id_to_parameter_block.end(); ++it) { + options.ordering.insert(options.ordering.end(), + it->second.begin(), + it->second.end()); + } + CHECK_EQ(options.ordering.size(), + original_problem_impl->NumParameterBlocks()); + } + } + } else { + CHECK(options.ordering_new_api == NULL); + } + + Program* original_program = original_problem_impl->mutable_program(); ProblemImpl* problem_impl = original_problem_impl; // Reset the summary object to its default values.