include/ceres/ordered_groups.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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 <vector>
  37 #include "ceres/internal/port.h"
  38 #include "glog/logging.h"
  39
  40 namespace ceres {
  41
  42 // A class for storing and manipulating an ordered collection of
  43 // groups/sets with the following semantics:
  44 //
  45 // Group ids are non-negative integer values. Elements are any type
  46 // that can serve as a key in a map or an element of a set.
  47 //
  48 // An element can only belong to one group at a time. A group may
  49 // contain an arbitrary number of elements.
  50 //
  51 // Groups are ordered by their group id.
  52 template <typename T>
  53 class OrderedGroups {
  54  public:
  55   // Add an element to a group. If a group with this id does not
  56   // exist, one is created. This method can be called any number of
  57   // times for the same element. Group ids should be non-negative
  58   // numbers.
  59   //
  60   // Return value indicates if adding the element was a success.
  61   bool AddElementToGroup(const T element, const int group) {
  62     if (group < 0) {
  63       return false;
  64     }
  65
  66     typename std::map<T, int>::const_iterator it =
  67         element_to_group_.find(element);
  68     if (it != element_to_group_.end()) {
  69       if (it->second == group) {
  70         // Element is already in the right group, nothing to do.
  71         return true;
  72       }
  73
  74       group_to_elements_[it->second].erase(element);
  75       if (group_to_elements_[it->second].size() == 0) {
  76         group_to_elements_.erase(it->second);
  77       }
  78     }
  79
  80     element_to_group_[element] = group;
  81     group_to_elements_[group].insert(element);
  82     return true;
  83   }
  84
  85   void Clear() {
  86     group_to_elements_.clear();
  87     element_to_group_.clear();
  88   }
  89
  90   // Remove the element, no matter what group it is in. Return value
  91   // indicates if the element was actually removed.
  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   // Bulk remove elements. The return value indicates the number of
 110   // elements successfully removed.
 111   int Remove(const std::vector<T>& elements) {
 112     if (NumElements() == 0 || elements.size() == 0) {
 113       return 0;
 114     }
 115
 116     int num_removed = 0;
 117     for (int i = 0; i < elements.size(); ++i) {
 118       num_removed += Remove(elements[i]);
 119     }
 120     return num_removed;
 121   }
 122
 123   // Reverse the order of the groups in place.
 124   void Reverse() {
 125     if (NumGroups() == 0) {
 126       return;
 127     }
 128
 129     typename std::map<int, std::set<T> >::reverse_iterator it =
 130         group_to_elements_.rbegin();
 131     std::map<int, std::set<T> > new_group_to_elements;
 132     new_group_to_elements[it->first] = it->second;
 133
 134     int new_group_id = it->first + 1;
 135     for (++it; it != group_to_elements_.rend(); ++it) {
 136       for (typename std::set<T>::const_iterator element_it = it->second.begin();
 137            element_it != it->second.end();
 138            ++element_it) {
 139         element_to_group_[*element_it] = new_group_id;
 140       }
 141       new_group_to_elements[new_group_id] = it->second;
 142       new_group_id++;
 143     }
 144
 145     group_to_elements_.swap(new_group_to_elements);
 146   }
 147
 148   // Return the group id for the element. If the element is not a
 149   // member of any group, return -1.
 150   int GroupId(const T element) const {
 151     typename std::map<T, int>::const_iterator it =
 152         element_to_group_.find(element);
 153     if (it == element_to_group_.end()) {
 154       return -1;
 155     }
 156     return it->second;
 157   }
 158
 159   bool IsMember(const T element) const {
 160     typename std::map<T, int>::const_iterator it =
 161         element_to_group_.find(element);
 162     return (it != element_to_group_.end());
 163   }
 164
 165   // This function always succeeds, i.e., implicitly there exists a
 166   // group for every integer.
 167   int GroupSize(const int group) const {
 168     typename std::map<int, std::set<T> >::const_iterator it =
 169         group_to_elements_.find(group);
 170     return (it ==  group_to_elements_.end()) ? 0 : it->second.size();
 171   }
 172
 173   int NumElements() const {
 174     return element_to_group_.size();
 175   }
 176
 177   // Number of groups with one or more elements.
 178   int NumGroups() const {
 179     return group_to_elements_.size();
 180   }
 181
 182   // The first group with one or more elements. Calling this when
 183   // there are no groups with non-zero elements will result in a
 184   // crash.
 185   int MinNonZeroGroup() const {
 186     CHECK_NE(NumGroups(), 0);
 187     return group_to_elements_.begin()->first;
 188   }
 189
 190   const std::map<int, std::set<T> >& group_to_elements() const {
 191     return group_to_elements_;
 192   }
 193
 194   const std::map<T, int>& element_to_group() const {
 195     return element_to_group_;
 196   }
 197
 198  private:
 199   std::map<int, std::set<T> > group_to_elements_;
 200   std::map<T, int> element_to_group_;
 201 };
 202
 203 // Typedef for the most commonly used version of OrderedGroups.
 204 typedef OrderedGroups<double*> ParameterBlockOrdering;
 205
 206 }  // namespace ceres
 207
 208 #endif  // CERES_PUBLIC_ORDERED_GROUP_H_