From ec3a3ceafccaf32b6a03bb386157171e1374cfac Mon Sep 17 00:00:00 2001
From: Sameer Agarwal <sameeragarwal@google.com>
Date: Thu, 29 May 2014 13:33:42 -0700
Subject: [PATCH] Flesh out OrderedGroups.

1. Add the ability to bulk remove elements.
2. Add accessor for elements_to_group.
3. Early exit in Remove if there are no elements.

Change-Id: I3df1f00de05338a42e9907423b674469a022d3bc
---
 include/ceres/ordered_groups.h        | 19 +++++++++++
 internal/ceres/ordered_groups_test.cc | 46 ++++++++++++++++++++++++---
 2 files changed, 60 insertions(+), 5 deletions(-)

diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h
index dff859d7b..1cbb7ff87 100644
--- a/include/ceres/ordered_groups.h
+++ b/include/ceres/ordered_groups.h
@@ -33,6 +33,7 @@
 
 #include <map>
 #include <set>
+#include <vector>
 #include "ceres/internal/port.h"
 
 namespace ceres {
@@ -103,6 +104,20 @@ class OrderedGroups {
     return true;
   }
 
+  // Bulk remove elements. The return value indicates the number of
+  // elements successfully removed.
+  int Remove(const vector<T>& elements) {
+    if (NumElements() == 0 || elements.size() == 0) {
+      return 0;
+    }
+
+    int num_removed = 0;
+    for (int i = 0; i < elements.size(); ++i) {
+      num_removed += Remove(elements[i]);
+    }
+    return num_removed;
+  }
+
   // Reverse the order of the groups in place.
   void Reverse() {
     typename map<int, set<T> >::reverse_iterator it =
@@ -160,6 +175,10 @@ class OrderedGroups {
     return group_to_elements_;
   }
 
+  const map<T, int>& element_to_group() const {
+    return element_to_group_;
+  }
+
  private:
   map<int, set<T> > group_to_elements_;
   map<T, int> element_to_group_;
diff --git a/internal/ceres/ordered_groups_test.cc b/internal/ceres/ordered_groups_test.cc
index 700e7881d..6b271a88e 100644
--- a/internal/ceres/ordered_groups_test.cc
+++ b/internal/ceres/ordered_groups_test.cc
@@ -38,7 +38,7 @@
 namespace ceres {
 namespace internal {
 
-TEST(OrderedGroup, EmptyOrderedGroupBehavesCorrectly) {
+TEST(OrderedGroups, EmptyOrderedGroupBehavesCorrectly) {
   ParameterBlockOrdering ordering;
   EXPECT_EQ(ordering.NumGroups(), 0);
   EXPECT_EQ(ordering.NumElements(), 0);
@@ -48,7 +48,7 @@ TEST(OrderedGroup, EmptyOrderedGroupBehavesCorrectly) {
   EXPECT_FALSE(ordering.Remove(&x));
 }
 
-TEST(OrderedGroup, EverythingInOneGroup) {
+TEST(OrderedGroups, EverythingInOneGroup) {
   ParameterBlockOrdering ordering;
   double x[3];
   ordering.AddElementToGroup(x, 1);
@@ -75,7 +75,7 @@ TEST(OrderedGroup, EverythingInOneGroup) {
   EXPECT_EQ(ordering.GroupId(x + 2), 1);
 }
 
-TEST(OrderedGroup, StartInOneGroupAndThenSplit) {
+TEST(OrderedGroups, StartInOneGroupAndThenSplit) {
   ParameterBlockOrdering ordering;
   double x[3];
   ordering.AddElementToGroup(x, 1);
@@ -103,7 +103,7 @@ TEST(OrderedGroup, StartInOneGroupAndThenSplit) {
   EXPECT_EQ(ordering.GroupId(x + 2), 1);
 }
 
-TEST(OrderedGroup, AddAndRemoveEveryThingFromOneGroup) {
+TEST(OrderedGroups, AddAndRemoveEveryThingFromOneGroup) {
   ParameterBlockOrdering ordering;
   double x[3];
   ordering.AddElementToGroup(x, 1);
@@ -133,7 +133,7 @@ TEST(OrderedGroup, AddAndRemoveEveryThingFromOneGroup) {
   EXPECT_EQ(ordering.GroupId(x + 2), 5);
 }
 
-TEST(OrderedGroup, ReverseOrdering) {
+TEST(OrderedGroups, ReverseOrdering) {
   ParameterBlockOrdering ordering;
   double x[3];
   ordering.AddElementToGroup(x, 1);
@@ -159,5 +159,41 @@ TEST(OrderedGroup, ReverseOrdering) {
   EXPECT_EQ(ordering.GroupId(x + 2), 2);
 }
 
+TEST(OrderedGroups, BulkRemove) {
+  ParameterBlockOrdering ordering;
+  double x[3];
+  ordering.AddElementToGroup(x, 1);
+  ordering.AddElementToGroup(x + 1, 2);
+  ordering.AddElementToGroup(x + 2, 2);
+
+  vector<double*> elements_to_remove;
+  elements_to_remove.push_back(x);
+  elements_to_remove.push_back(x + 2);
+
+  EXPECT_EQ(ordering.Remove(elements_to_remove), 2);
+  EXPECT_EQ(ordering.NumElements(), 1);
+  EXPECT_EQ(ordering.GroupId(x), -1);
+  EXPECT_EQ(ordering.GroupId(x + 1), 2);
+  EXPECT_EQ(ordering.GroupId(x + 2), -1);
+}
+
+TEST(OrderedGroups, BulkRemoveWithNoElements) {
+  ParameterBlockOrdering ordering;
+
+  double x[3];
+  vector<double*> elements_to_remove;
+  elements_to_remove.push_back(x);
+  elements_to_remove.push_back(x + 2);
+
+  EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
+
+  ordering.AddElementToGroup(x, 1);
+  ordering.AddElementToGroup(x + 1, 2);
+  ordering.AddElementToGroup(x + 2, 2);
+
+  elements_to_remove.clear();
+  EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
+}
+
 }  // namespace internal
 }  // namespace ceres