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.
