OrderedGroups implementation. This generalizes the Ordering object and paves the path for a more general inner iteration API. Change-Id: I6efce5f999c2bfab5f90a8a18e21140581f207cd
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