From e9fcf3decba3352a33857265b8dbf820fb1a43e6 Mon Sep 17 00:00:00 2001
From: Sameer Agarwal <sameeragarwal@google.com>
Date: Mon, 9 Jun 2014 12:03:17 -0700
Subject: [PATCH] Add OrderedGroups::MinNonZeroGroup.

Change-Id: If571c2435a7c884b472c33421722208cb2d036ff
---
 include/ceres/ordered_groups.h        |  9 +++++++++
 internal/ceres/ordered_groups_test.cc | 20 ++++++++++++++++++++
 internal/ceres/solver_impl.cc         |  2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h
index 1cbb7ff87..c316d712e 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 @@ class OrderedGroups {
     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 6b271a88e..7719d35cb 100644
--- a/internal/ceres/ordered_groups_test.cc
+++ b/internal/ceres/ordered_groups_test.cc
@@ -195,5 +195,25 @@ TEST(OrderedGroups, BulkRemoveWithNoElements) {
   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 a690eed0b..a4edf087d 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -694,7 +694,7 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options,
   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 =