diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h
new file mode 100644
index 0000000..5f41dd4
--- /dev/null
+++ b/include/ceres/ordered_groups.h
@@ -0,0 +1,146 @@
+// 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/collections_port.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// A class for storing and manipulating an ordered collection of
+// groups/sets with the following semantics:
+//
+// Group ids are non-negative integer values. Elements are any type
+// that can serve as a key in a map or an element of a set.
+//
+// An element can only belong to one group at a time. A group may
+// contain an arbitrary number of elements.
+//
+// Groups are ordered by their group id.
+template <typename T>
+class OrderedGroups {
+ public:
+  // Add an element to a group. If a group with this id does not
+  // exist, one is created. This method can be called any number of
+  // times for the same element. Group ids should be non-negative
+  // numbers.
+  //
+  // Return value indicates if adding the element was a success.
+  bool AddElementToGroup(const T element, const int group) {
+    if (group < 0) {
+      return false;
+    }
+
+    typename map<T, int>::const_iterator it = element_to_group_.find(element);
+    if (it != element_to_group_.end()) {
+      if (it->second == group) {
+        // Element is already in the right group, nothing to do.
+        return true;
+      }
+
+      group_to_elements_[it->second].erase(element);
+      if (group_to_elements_[it->second].size() == 0) {
+        group_to_elements_.erase(it->second);
+      }
+    }
+
+    element_to_group_[element] = group;
+    group_to_elements_[group].insert(element);
+    return true;
+  }
+
+  // Remove the element, no matter what group it is in. If the element
+  // is not a member of any group, calling this method will result in
+  // a crash.
+  //
+  // Return value indicates if the element was actually removed.
+  bool Remove(const T element) {
+    const int current_group = GroupId(element);
+    if (current_group < 0) {
+      return false;
+    }
+
+    group_to_elements_[current_group].erase(element);
+
+    if (group_to_elements_[current_group].size() == 0) {
+      // If the group is empty, then get rid of it.
+      group_to_elements_.erase(current_group);
+    }
+
+    element_to_group_.erase(element);
+    return true;
+  }
+
+  // Return the group id for the element. If the element is not a
+  // member of any group, return -1.
+  int GroupId(const T element) const {
+    typename map<T, int>::const_iterator it = element_to_group_.find(element);
+    if (it == element_to_group_.end()) {
+      return -1;
+    }
+    return it->second;
+  }
+
+  bool IsMember(const T element) const {
+    typename map<T, int>::const_iterator it = element_to_group_.find(element);
+    return (it != element_to_group_.end());
+  }
+
+  // This function always succeeds, i.e., implicitly there exists a
+  // group for every integer.
+  int GroupSize(const int group) const {
+    typename map<int, set<T> >::const_iterator it =
+        group_to_elements_.find(group);
+    return (it ==  group_to_elements_.end()) ? 0 : it->second.size();
+  }
+
+  int NumElements() const {
+    return element_to_group_.size();
+  }
+
+  // Number of groups with one or more elements.
+  int NumGroups() const {
+    return group_to_elements_.size();
+  }
+
+  const map<int, set<T> > group_to_elements() const {
+    return group_to_elements_;
+  }
+
+ private:
+  map<int, set<T> > group_to_elements_;
+  map<T, int> element_to_group_;
+};
+
+// Typedef for the most commonly used version of OrderedGroups.
+typedef OrderedGroups<double*> ParameterBlockOrdering;
+
+}  // namespace ceres
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index e094f6a..e0e8afa 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -235,6 +235,7 @@
   CERES_TEST(normal_prior)
   CERES_TEST(numeric_diff_cost_function)
   CERES_TEST(ordering)
+  CERES_TEST(ordered_groups)
   CERES_TEST(parameter_block)
   CERES_TEST(partitioned_matrix_view)
   CERES_TEST(polynomial_solver)
diff --git a/internal/ceres/ordered_groups_test.cc b/internal/ceres/ordered_groups_test.cc
new file mode 100644
index 0000000..214d032
--- /dev/null
+++ b/internal/ceres/ordered_groups_test.cc
@@ -0,0 +1,137 @@
+// 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/ordered_groups.h"
+
+#include <cstddef>
+#include <vector>
+#include "gtest/gtest.h"
+#include "ceres/collections_port.h"
+
+namespace ceres {
+namespace internal {
+
+TEST(OrderedGroup, EmptyOrderedGroupBehavesCorrectly) {
+  ParameterBlockOrdering ordering;
+  EXPECT_EQ(ordering.NumGroups(), 0);
+  EXPECT_EQ(ordering.NumElements(), 0);
+  EXPECT_EQ(ordering.GroupSize(1), 0);
+  double x;
+  EXPECT_EQ(ordering.GroupId(&x), -1);
+  EXPECT_FALSE(ordering.Remove(&x));
+}
+
+TEST(OrderedGroup, EverythingInOneGroup) {
+  ParameterBlockOrdering ordering;
+  double x[3];
+  ordering.AddElementToGroup(x, 1);
+  ordering.AddElementToGroup(x + 1, 1);
+  ordering.AddElementToGroup(x + 2, 1);
+  ordering.AddElementToGroup(x, 1);
+
+  EXPECT_EQ(ordering.NumGroups(), 1);
+  EXPECT_EQ(ordering.NumElements(), 3);
+  EXPECT_EQ(ordering.GroupSize(1), 3);
+  EXPECT_EQ(ordering.GroupSize(0), 0);
+  EXPECT_EQ(ordering.GroupId(x), 1);
+  EXPECT_EQ(ordering.GroupId(x + 1), 1);
+  EXPECT_EQ(ordering.GroupId(x + 2), 1);
+
+  ordering.Remove(x);
+  EXPECT_EQ(ordering.NumGroups(), 1);
+  EXPECT_EQ(ordering.NumElements(), 2);
+  EXPECT_EQ(ordering.GroupSize(1), 2);
+  EXPECT_EQ(ordering.GroupSize(0), 0);
+
+  EXPECT_EQ(ordering.GroupId(x), -1);
+  EXPECT_EQ(ordering.GroupId(x + 1), 1);
+  EXPECT_EQ(ordering.GroupId(x + 2), 1);
+}
+
+TEST(OrderedGroup, StartInOneGroupAndThenSplit) {
+  ParameterBlockOrdering ordering;
+  double x[3];
+  ordering.AddElementToGroup(x, 1);
+  ordering.AddElementToGroup(x + 1, 1);
+  ordering.AddElementToGroup(x + 2, 1);
+  ordering.AddElementToGroup(x, 1);
+
+  EXPECT_EQ(ordering.NumGroups(), 1);
+  EXPECT_EQ(ordering.NumElements(), 3);
+  EXPECT_EQ(ordering.GroupSize(1), 3);
+  EXPECT_EQ(ordering.GroupSize(0), 0);
+  EXPECT_EQ(ordering.GroupId(x), 1);
+  EXPECT_EQ(ordering.GroupId(x + 1), 1);
+  EXPECT_EQ(ordering.GroupId(x + 2), 1);
+
+  ordering.AddElementToGroup(x, 5);
+  EXPECT_EQ(ordering.NumGroups(), 2);
+  EXPECT_EQ(ordering.NumElements(), 3);
+  EXPECT_EQ(ordering.GroupSize(1), 2);
+  EXPECT_EQ(ordering.GroupSize(5), 1);
+  EXPECT_EQ(ordering.GroupSize(0), 0);
+
+  EXPECT_EQ(ordering.GroupId(x), 5);
+  EXPECT_EQ(ordering.GroupId(x + 1), 1);
+  EXPECT_EQ(ordering.GroupId(x + 2), 1);
+}
+
+TEST(OrderedGroup, AddAndRemoveEveryThingFromOneGroup) {
+  ParameterBlockOrdering ordering;
+  double x[3];
+  ordering.AddElementToGroup(x, 1);
+  ordering.AddElementToGroup(x + 1, 1);
+  ordering.AddElementToGroup(x + 2, 1);
+  ordering.AddElementToGroup(x, 1);
+
+  EXPECT_EQ(ordering.NumGroups(), 1);
+  EXPECT_EQ(ordering.NumElements(), 3);
+  EXPECT_EQ(ordering.GroupSize(1), 3);
+  EXPECT_EQ(ordering.GroupSize(0), 0);
+  EXPECT_EQ(ordering.GroupId(x), 1);
+  EXPECT_EQ(ordering.GroupId(x + 1), 1);
+  EXPECT_EQ(ordering.GroupId(x + 2), 1);
+
+  ordering.AddElementToGroup(x, 5);
+  ordering.AddElementToGroup(x + 1, 5);
+  ordering.AddElementToGroup(x + 2, 5);
+  EXPECT_EQ(ordering.NumGroups(), 1);
+  EXPECT_EQ(ordering.NumElements(), 3);
+  EXPECT_EQ(ordering.GroupSize(1), 0);
+  EXPECT_EQ(ordering.GroupSize(5), 3);
+  EXPECT_EQ(ordering.GroupSize(0), 0);
+
+  EXPECT_EQ(ordering.GroupId(x), 5);
+  EXPECT_EQ(ordering.GroupId(x + 1), 5);
+  EXPECT_EQ(ordering.GroupId(x + 2), 5);
+}
+
+}  // namespace internal
+}  // namespace ceres