Add OrderedGroups::MinNonZeroGroup.

Change-Id: If571c2435a7c884b472c33421722208cb2d036ff

include/ceres/ordered_groups.h

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_;
   }

internal/ceres/ordered_groups_test.cc

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

internal/ceres/solver_impl.cc

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 =