1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: sameeragarwal@google.com (Sameer Agarwal) 30 31 #ifndef CERES_PUBLIC_ORDERED_GROUPS_H_ 32 #define CERES_PUBLIC_ORDERED_GROUPS_H_ 33 34 #include <map> 35 #include <set> 36 #include "ceres/internal/port.h" 37 38 namespace ceres { 39 40 // A class for storing and manipulating an ordered collection of 41 // groups/sets with the following semantics: 42 // 43 // Group ids are non-negative integer values. Elements are any type 44 // that can serve as a key in a map or an element of a set. 45 // 46 // An element can only belong to one group at a time. A group may 47 // contain an arbitrary number of elements. 48 // 49 // Groups are ordered by their group id. 50 template <typename T> 51 class OrderedGroups { 52 public: 53 // Add an element to a group. If a group with this id does not 54 // exist, one is created. This method can be called any number of 55 // times for the same element. Group ids should be non-negative 56 // numbers. 57 // 58 // Return value indicates if adding the element was a success. AddElementToGroup(const T element,const int group)59 bool AddElementToGroup(const T element, const int group) { 60 if (group < 0) { 61 return false; 62 } 63 64 typename map<T, int>::const_iterator it = element_to_group_.find(element); 65 if (it != element_to_group_.end()) { 66 if (it->second == group) { 67 // Element is already in the right group, nothing to do. 68 return true; 69 } 70 71 group_to_elements_[it->second].erase(element); 72 if (group_to_elements_[it->second].size() == 0) { 73 group_to_elements_.erase(it->second); 74 } 75 } 76 77 element_to_group_[element] = group; 78 group_to_elements_[group].insert(element); 79 return true; 80 } 81 Clear()82 void Clear() { 83 group_to_elements_.clear(); 84 element_to_group_.clear(); 85 } 86 87 // Remove the element, no matter what group it is in. If the element 88 // is not a member of any group, calling this method will result in 89 // a crash. 90 // 91 // Return value indicates if the element was actually removed. Remove(const T element)92 bool Remove(const T element) { 93 const int current_group = GroupId(element); 94 if (current_group < 0) { 95 return false; 96 } 97 98 group_to_elements_[current_group].erase(element); 99 100 if (group_to_elements_[current_group].size() == 0) { 101 // If the group is empty, then get rid of it. 102 group_to_elements_.erase(current_group); 103 } 104 105 element_to_group_.erase(element); 106 return true; 107 } 108 109 // Reverse the order of the groups in place. Reverse()110 void Reverse() { 111 typename map<int, set<T> >::reverse_iterator it = 112 group_to_elements_.rbegin(); 113 map<int, set<T> > new_group_to_elements; 114 new_group_to_elements[it->first] = it->second; 115 116 int new_group_id = it->first + 1; 117 for (++it; it != group_to_elements_.rend(); ++it) { 118 for (typename set<T>::const_iterator element_it = it->second.begin(); 119 element_it != it->second.end(); 120 ++element_it) { 121 element_to_group_[*element_it] = new_group_id; 122 } 123 new_group_to_elements[new_group_id] = it->second; 124 new_group_id++; 125 } 126 127 group_to_elements_.swap(new_group_to_elements); 128 } 129 130 // Return the group id for the element. If the element is not a 131 // member of any group, return -1. GroupId(const T element)132 int GroupId(const T element) const { 133 typename map<T, int>::const_iterator it = element_to_group_.find(element); 134 if (it == element_to_group_.end()) { 135 return -1; 136 } 137 return it->second; 138 } 139 IsMember(const T element)140 bool IsMember(const T element) const { 141 typename map<T, int>::const_iterator it = element_to_group_.find(element); 142 return (it != element_to_group_.end()); 143 } 144 145 // This function always succeeds, i.e., implicitly there exists a 146 // group for every integer. GroupSize(const int group)147 int GroupSize(const int group) const { 148 typename map<int, set<T> >::const_iterator it = 149 group_to_elements_.find(group); 150 return (it == group_to_elements_.end()) ? 0 : it->second.size(); 151 } 152 NumElements()153 int NumElements() const { 154 return element_to_group_.size(); 155 } 156 157 // Number of groups with one or more elements. NumGroups()158 int NumGroups() const { 159 return group_to_elements_.size(); 160 } 161 group_to_elements()162 const map<int, set<T> >& group_to_elements() const { 163 return group_to_elements_; 164 } 165 166 private: 167 map<int, set<T> > group_to_elements_; 168 map<T, int> element_to_group_; 169 }; 170 171 // Typedef for the most commonly used version of OrderedGroups. 172 typedef OrderedGroups<double*> ParameterBlockOrdering; 173 174 } // namespace ceres 175 176 #endif // CERES_PUBLIC_ORDERED_GROUP_H_ 177