Add OrderedGroups::MinNonZeroGroup.
Change-Id: If571c2435a7c884b472c33421722208cb2d036ff
diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h
index 1cbb7ff..c316d71 100644
--- a/include/ceres/ordered_groups.h
+++ b/include/ceres/ordered_groups.h
@@ -35,6 +35,7 @@
#include <set>
#include <vector>
#include "ceres/internal/port.h"
+#include "glog/logging.h"
namespace ceres {
@@ -171,6 +172,14 @@
return group_to_elements_.size();
}
+ // The first group with one or more elements. Calling this when
+ // there are no groups with non-zero elements will result in a
+ // crash.
+ int MinNonZeroGroup() const {
+ CHECK_NE(NumGroups(), 0);
+ return group_to_elements_.begin()->first;
+ }
+
const map<int, set<T> >& group_to_elements() const {
return group_to_elements_;
}
diff --git a/internal/ceres/ordered_groups_test.cc b/internal/ceres/ordered_groups_test.cc
index 6b271a8..7719d35 100644
--- a/internal/ceres/ordered_groups_test.cc
+++ b/internal/ceres/ordered_groups_test.cc
@@ -195,5 +195,25 @@
EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
}
+TEST(OrderedGroups, MinNonZeroGroup) {
+ ParameterBlockOrdering ordering;
+ double x[3];
+
+ ordering.AddElementToGroup(x, 1);
+ ordering.AddElementToGroup(x + 1, 1);
+ ordering.AddElementToGroup(x + 2, 2);
+
+ EXPECT_EQ(ordering.MinNonZeroGroup(), 1);
+ ordering.Remove(x);
+
+ EXPECT_EQ(ordering.MinNonZeroGroup(), 1);
+ ordering.Remove(x + 1);
+
+ EXPECT_EQ(ordering.MinNonZeroGroup(), 2);
+ ordering.Remove(x + 2);
+
+ // No non-zero groups left.
+ EXPECT_DEATH_IF_SUPPORTED(ordering.MinNonZeroGroup(), "NumGroups()");
+}
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index a690eed..a4edf08 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -694,7 +694,7 @@
ParameterBlockOrdering* linear_solver_ordering =
options->linear_solver_ordering.get();
const int min_group_id =
- linear_solver_ordering->group_to_elements().begin()->first;
+ linear_solver_ordering->MinNonZeroGroup();
linear_solver_ordering->Remove(removed_parameter_blocks);
ParameterBlockOrdering* inner_iteration_ordering =
