10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_PUBLIC_ORDERED_GROUPS_H_ 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_PUBLIC_ORDERED_GROUPS_H_ 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <map> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <set> 3679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include <vector> 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 3879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "glog/logging.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A class for storing and manipulating an ordered collection of 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// groups/sets with the following semantics: 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Group ids are non-negative integer values. Elements are any type 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// that can serve as a key in a map or an element of a set. 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// An element can only belong to one group at a time. A group may 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// contain an arbitrary number of elements. 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Groups are ordered by their group id. 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass OrderedGroups { 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add an element to a group. If a group with this id does not 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // exist, one is created. This method can be called any number of 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // times for the same element. Group ids should be non-negative 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // numbers. 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Return value indicates if adding the element was a success. 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool AddElementToGroup(const T element, const int group) { 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (group < 0) { 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typename map<T, int>::const_iterator it = element_to_group_.find(element); 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (it != element_to_group_.end()) { 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (it->second == group) { 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Element is already in the right group, nothing to do. 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_[it->second].erase(element); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (group_to_elements_[it->second].size() == 0) { 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_.erase(it->second); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong element_to_group_[element] = group; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_[group].insert(element); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void Clear() { 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_.clear(); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong element_to_group_.clear(); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Remove the element, no matter what group it is in. Return value 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // indicates if the element was actually removed. 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool Remove(const T element) { 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int current_group = GroupId(element); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (current_group < 0) { 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_[current_group].erase(element); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (group_to_elements_[current_group].size() == 0) { 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the group is empty, then get rid of it. 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_.erase(current_group); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong element_to_group_.erase(element); 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Bulk remove elements. The return value indicates the number of 10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // elements successfully removed. 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int Remove(const vector<T>& elements) { 11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (NumElements() == 0 || elements.size() == 0) { 11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return 0; 11379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 11479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 11579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_removed = 0; 11679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < elements.size(); ++i) { 11779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez num_removed += Remove(elements[i]); 11879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return num_removed; 12079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 12179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Reverse the order of the groups in place. 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void Reverse() { 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typename map<int, set<T> >::reverse_iterator it = 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_.rbegin(); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong map<int, set<T> > new_group_to_elements; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_group_to_elements[it->first] = it->second; 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int new_group_id = it->first + 1; 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (++it; it != group_to_elements_.rend(); ++it) { 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (typename set<T>::const_iterator element_it = it->second.begin(); 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong element_it != it->second.end(); 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++element_it) { 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong element_to_group_[*element_it] = new_group_id; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_group_to_elements[new_group_id] = it->second; 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_group_id++; 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_.swap(new_group_to_elements); 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Return the group id for the element. If the element is not a 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // member of any group, return -1. 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int GroupId(const T element) const { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typename map<T, int>::const_iterator it = element_to_group_.find(element); 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (it == element_to_group_.end()) { 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return -1; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return it->second; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool IsMember(const T element) const { 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typename map<T, int>::const_iterator it = element_to_group_.find(element); 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (it != element_to_group_.end()); 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This function always succeeds, i.e., implicitly there exists a 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // group for every integer. 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int GroupSize(const int group) const { 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typename map<int, set<T> >::const_iterator it = 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong group_to_elements_.find(group); 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (it == group_to_elements_.end()) ? 0 : it->second.size(); 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int NumElements() const { 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return element_to_group_.size(); 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Number of groups with one or more elements. 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int NumGroups() const { 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return group_to_elements_.size(); 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 17579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The first group with one or more elements. Calling this when 17679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // there are no groups with non-zero elements will result in a 17779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // crash. 17879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int MinNonZeroGroup() const { 17979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NE(NumGroups(), 0); 18079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return group_to_elements_.begin()->first; 18179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 18279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const map<int, set<T> >& group_to_elements() const { 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return group_to_elements_; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const map<T, int>& element_to_group() const { 18879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return element_to_group_; 18979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 19079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong map<int, set<T> > group_to_elements_; 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong map<T, int> element_to_group_; 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Typedef for the most commonly used version of OrderedGroups. 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtypedef OrderedGroups<double*> ParameterBlockOrdering; 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_PUBLIC_ORDERED_GROUP_H_ 202