10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2013 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) 3079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// mierle@gmail.com (Keir Mierle) 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/problem_impl.h" 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstddef> 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <iterator> 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <set> 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <string> 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <utility> 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/casts.h" 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/compressed_row_sparse_matrix.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/cost_function.h" 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/crs_matrix.h" 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/evaluator.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/loss_function.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/map_util.h" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/parameter_block.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/program.h" 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/residual_block.h" 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stl_util.h" 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stringprintf.h" 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtypedef map<double*, internal::ParameterBlock*> ParameterMap; 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace { 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginternal::ParameterBlock* FindParameterBlockOrDie( 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const ParameterMap& parameter_map, 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* parameter_block) { 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterMap::const_iterator it = parameter_map.find(parameter_block); 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK(it != parameter_map.end()) 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Parameter block not found: " << parameter_block; 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return it->second; 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Returns true if two regions of memory, a and b, with sizes size_a and size_b 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// respectively, overlap. 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool RegionsAlias(const double* a, int size_a, 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* b, int size_b) { 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (a < b) ? b < (a + size_a) 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : a < (b + size_b); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid CheckForNoAliasing(double* existing_block, 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int existing_block_size, 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* new_block, 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int new_block_size) { 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK(!RegionsAlias(existing_block, existing_block_size, 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new_block, new_block_size)) 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "Aliasing detected between existing parameter block at memory " 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "location " << existing_block 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << " and has size " << existing_block_size << " with new parameter " 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "block that has memory address " << new_block << " and would have " 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong << "size " << new_block_size << "."; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int size) { 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK(values != NULL) << "Null pointer passed to AddParameterBlock " 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "for a parameter with size " << size; 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Ignore the request if there is a block for the given pointer already. 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterMap::iterator it = parameter_block_map_.find(values); 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (it != parameter_block_map_.end()) { 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!options_.disable_all_safety_checks) { 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int existing_size = it->second->Size(); 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK(size == existing_size) 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Tried adding a parameter block with the same double pointer, " 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << values << ", twice, but with different block sizes. Original " 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "size was " << existing_size << " but new size is " 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << size; 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return it->second; 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!options_.disable_all_safety_checks) { 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Before adding the parameter block, also check that it doesn't alias any 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // other parameter blocks. 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!parameter_block_map_.empty()) { 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterMap::iterator lb = parameter_block_map_.lower_bound(values); 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If lb is not the first block, check the previous block for aliasing. 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (lb != parameter_block_map_.begin()) { 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterMap::iterator previous = lb; 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling --previous; 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckForNoAliasing(previous->first, 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling previous->second->Size(), 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values, 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling size); 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If lb is not off the end, check lb for aliasing. 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (lb != parameter_block_map_.end()) { 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CheckForNoAliasing(lb->first, 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling lb->second->Size(), 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values, 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling size); 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Pass the index of the new parameter block as well to keep the index in 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // sync with the position of the parameter in the program's parameter vector. 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterBlock* new_parameter_block = 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new ParameterBlock(values, size, program_->parameter_blocks_.size()); 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // For dynamic problems, add the list of dependent residual blocks, which is 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // empty to start. 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new_parameter_block->EnableResidualBlockDependencies(); 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_map_[values] = new_parameter_block; 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling program_->parameter_blocks_.push_back(new_parameter_block); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return new_parameter_block; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(residual_block); 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Perform no check on the validity of residual_block, that is handled in 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // the public method: RemoveResidualBlock(). 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // If needed, remove the parameter dependencies on this residual block. 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_parameter_blocks_for_residual = 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block->NumParameterBlocks(); 16279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_parameter_blocks_for_residual; ++i) { 16379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block->parameter_blocks()[i] 16479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ->RemoveResidualBlock(residual_block); 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlockSet::iterator it = residual_block_set_.find(residual_block); 16879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block_set_.erase(it); 16979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 17079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); 17179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 17279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Deletes the residual block in question, assuming there are no other 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// references to it inside the problem (e.g. by another parameter). Referenced 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cost and loss functions are tucked away for future deletion, since it is not 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// possible to know whether other parts of the problem depend on them without 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// doing a full scan. 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The const casts here are legit, since ResidualBlock holds these 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // pointers as const pointers but we have ownership of them and 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // have the right to destroy them when the destructor is called. 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (options_.cost_function_ownership == TAKE_OWNERSHIP && 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual_block->cost_function() != NULL) { 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_functions_to_delete_.push_back( 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const_cast<CostFunction*>(residual_block->cost_function())); 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (options_.loss_function_ownership == TAKE_OWNERSHIP && 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual_block->loss_function() != NULL) { 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling loss_functions_to_delete_.push_back( 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const_cast<LossFunction*>(residual_block->loss_function())); 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling delete residual_block; 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Deletes the parameter block in question, assuming there are no other 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// references to it inside the problem (e.g. by any residual blocks). 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Referenced parameterizations are tucked away for future deletion, since it 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// is not possible to know whether other parts of the problem depend on them 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// without doing a full scan. 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block->local_parameterization() != NULL) { 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling local_parameterizations_to_delete_.push_back( 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block->mutable_local_parameterization()); 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_map_.erase(parameter_block->mutable_user_state()); 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling delete parameter_block; 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongProblemImpl::ProblemImpl() : program_(new internal::Program) {} 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongProblemImpl::ProblemImpl(const Problem::Options& options) 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : options_(options), 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong program_(new internal::Program) {} 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongProblemImpl::~ProblemImpl() { 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Collect the unique cost/loss functions and delete the residuals. 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_residual_blocks = program_->residual_blocks_.size(); 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_functions_to_delete_.reserve(num_residual_blocks); 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling loss_functions_to_delete_.reserve(num_residual_blocks); 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < program_->residual_blocks_.size(); ++i) { 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DeleteBlock(program_->residual_blocks_[i]); 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Collect the unique parameterizations and delete the parameters. 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DeleteBlock(program_->parameter_blocks_[i]); 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Delete the owned cost/loss functions and parameterizations. 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling local_parameterizations_to_delete_.end()); 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(), 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_functions_to_delete_.end()); 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(), 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling loss_functions_to_delete_.end()); 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<double*>& parameter_blocks) { 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(cost_function); 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(parameter_blocks.size(), 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cost_function->parameter_block_sizes().size()); 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Check the sizes match. 24779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int32>& parameter_block_sizes = 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cost_function->parameter_block_sizes(); 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!options_.disable_all_safety_checks) { 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Number of blocks input is different than the number of blocks " 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "that the cost function expects."; 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check for duplicate parameter blocks. 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*> sorted_parameter_blocks(parameter_blocks); 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double*>::const_iterator duplicate_items = 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling unique(sorted_parameter_blocks.begin(), 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling sorted_parameter_blocks.end()); 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (duplicate_items != sorted_parameter_blocks.end()) { 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string blocks; 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < parameter_blocks.size(); ++i) { 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling blocks += StringPrintf(" %p ", parameter_blocks[i]); 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "are not allowed. Parameter block pointers: [" 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << blocks << "]"; 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add parameter blocks and convert the double*'s to parameter blocks. 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks.size(); ++i) { 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_block_ptrs[i] = 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong InternalAddParameterBlock(parameter_blocks[i], 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_sizes[i]); 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!options_.disable_all_safety_checks) { 2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Check that the block sizes match the block sizes expected by the 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // cost_function. 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < parameter_block_ptrs.size(); ++i) { 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(cost_function->parameter_block_sizes()[i], 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_ptrs[i]->Size()) 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "The cost function expects parameter block " << i 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " of size " << cost_function->parameter_block_sizes()[i] 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " but was given a block of size " 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << parameter_block_ptrs[i]->Size(); 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ResidualBlock* new_residual_block = 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new ResidualBlock(cost_function, 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong loss_function, 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_ptrs, 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling program_->residual_blocks_.size()); 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Add dependencies on the residual to the parameter blocks. 30179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < parameter_blocks.size(); ++i) { 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_ptrs[i]->AddResidualBlock(new_residual_block); 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong program_->residual_blocks_.push_back(new_residual_block); 30879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 31079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block_set_.insert(new_residual_block); 31179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 31279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return new_residual_block; 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Unfortunately, macros don't help much to reduce this code, and var args don't 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// work because of the ambiguous case that there is no loss function. 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0) { 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1) { 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2) { 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3) { 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3, double* x4) { 3640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 3670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 3680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x4); 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 3710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { 3770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 3810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 3820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x4); 3830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x5); 3840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 3850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 3880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 3890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 3900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 3910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x6) { 3920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 3930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 3940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 3950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 3960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x4); 3980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x5); 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x6); 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 4060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 4070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x6, double* x7) { 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 4100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 4110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 4120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 4130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x4); 4140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x5); 4150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x6); 4160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x7); 4170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 4180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 4210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 4220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 4230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 4240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x6, double* x7, double* x8) { 4250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 4260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 4270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 4280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 4300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x4); 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x5); 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x6); 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x7); 4340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x8); 4350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingResidualBlock* ProblemImpl::AddResidualBlock( 4390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CostFunction* cost_function, 4400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LossFunction* loss_function, 4410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 4420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x6, double* x7, double* x8, double* x9) { 4430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<double*> residual_parameters; 4440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x0); 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x1); 4460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x2); 4470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x3); 4480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x4); 4490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x5); 4500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x6); 4510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x7); 4520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x8); 4530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_parameters.push_back(x9); 4540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return AddResidualBlock(cost_function, loss_function, residual_parameters); 4550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ProblemImpl::AddParameterBlock(double* values, int size) { 4581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling InternalAddParameterBlock(values, size); 4590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ProblemImpl::AddParameterBlock( 4620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* values, 4630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int size, 4640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LocalParameterization* local_parameterization) { 4650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ParameterBlock* parameter_block = 4661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling InternalAddParameterBlock(values, size); 4670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (local_parameterization != NULL) { 4680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_block->SetParameterization(local_parameterization); 4690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Delete a block from a vector of blocks, maintaining the indexing invariant. 4731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This is done in constant time by moving an element from the end of the 4741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// vector over the element to remove, then popping the last element. It 4751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// destroys the ordering in the interest of speed. 4761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename Block> 4771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, 4781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Block* block_to_remove) { 4791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) 48079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "You found a Ceres bug! \n" 48179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Block requested: " 48279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << block_to_remove->ToString() << "\n" 48379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Block present: " 48479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << (*mutable_blocks)[block_to_remove->index()]->ToString(); 4851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Prepare the to-be-moved block for the new, lower-in-index position by 4871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // setting the index to the blocks final location. 4881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Block* tmp = mutable_blocks->back(); 4891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling tmp->set_index(block_to_remove->index()); 4901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Overwrite the to-be-deleted residual block with the one at the end. 4921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*mutable_blocks)[block_to_remove->index()] = tmp; 4931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DeleteBlock(block_to_remove); 4951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The block is gone so shrink the vector of blocks accordingly. 4971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling mutable_blocks->pop_back(); 4981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { 5011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_NOTNULL(residual_block); 5021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 50379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Verify that residual_block identifies a residual in the current problem. 50479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const string residual_not_found_message = 50579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez StringPrintf("Residual block to remove: %p not found. This usually means " 50679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "one of three things have happened:\n" 50779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez " 1) residual_block is uninitialised and points to a random " 50879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "area in memory.\n" 50979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez " 2) residual_block represented a residual that was added to" 51079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez " the problem, but referred to a parameter block which has " 51179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "since been removed, which removes all residuals which " 51279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "depend on that parameter block, and was thus removed.\n" 51379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez " 3) residual_block referred to a residual that has already " 51479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "been removed from the problem (by the user).", 51579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block); 51679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 51779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK(residual_block_set_.find(residual_block) != 51879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block_set_.end()) 51979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << residual_not_found_message; 52079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else { 52179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Perform a full search over all current residuals. 52279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK(std::find(program_->residual_blocks().begin(), 52379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program_->residual_blocks().end(), 52479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block) != program_->residual_blocks().end()) 52579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << residual_not_found_message; 5261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 52779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 52879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez InternalRemoveResidualBlock(residual_block); 5291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ProblemImpl::RemoveParameterBlock(double* values) { 5321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterBlock* parameter_block = 5331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindParameterBlockOrDie(parameter_block_map_, values); 5341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 53579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 5361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Copy the dependent residuals from the parameter block because the set of 5371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // dependents will change after each call to RemoveResidualBlock(). 5381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ResidualBlock*> residual_blocks_to_remove( 5391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block->mutable_residual_blocks()->begin(), 5401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block->mutable_residual_blocks()->end()); 5411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < residual_blocks_to_remove.size(); ++i) { 54279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez InternalRemoveResidualBlock(residual_blocks_to_remove[i]); 5431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 5451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Scan all the residual blocks to remove ones that depend on the parameter 5461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // block. Do the scan backwards since the vector changes while iterating. 5471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_residual_blocks = NumResidualBlocks(); 5481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = num_residual_blocks - 1; i >= 0; --i) { 5491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ResidualBlock* residual_block = 5501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (*(program_->mutable_residual_blocks()))[i]; 5511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_parameter_blocks = residual_block->NumParameterBlocks(); 5521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < num_parameter_blocks; ++j) { 5531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (residual_block->parameter_blocks()[j] == parameter_block) { 55479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez InternalRemoveResidualBlock(residual_block); 5551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The parameter blocks are guaranteed unique. 5561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling break; 5571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block); 5621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ProblemImpl::SetParameterBlockConstant(double* values) { 5651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindParameterBlockOrDie(parameter_block_map_, values)->SetConstant(); 5660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ProblemImpl::SetParameterBlockVariable(double* values) { 5691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindParameterBlockOrDie(parameter_block_map_, values)->SetVarying(); 5700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ProblemImpl::SetParameterization( 5730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* values, 5740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LocalParameterization* local_parameterization) { 5751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindParameterBlockOrDie(parameter_block_map_, values) 5760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ->SetParameterization(local_parameterization); 5770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 57979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezconst LocalParameterization* ProblemImpl::GetParameterization( 58079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* values) const { 58179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return FindParameterBlockOrDie(parameter_block_map_, values) 58279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ->local_parameterization(); 58379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 58479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 58579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ProblemImpl::SetParameterLowerBound(double* values, 58679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int index, 58779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double lower_bound) { 58879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez FindParameterBlockOrDie(parameter_block_map_, values) 58979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ->SetLowerBound(index, lower_bound); 59079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 59179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 59279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ProblemImpl::SetParameterUpperBound(double* values, 59379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int index, 59479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double upper_bound) { 59579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez FindParameterBlockOrDie(parameter_block_map_, values) 59679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ->SetUpperBound(index, upper_bound); 59779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 59879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, 6001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* cost, 6011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double>* residuals, 6021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double>* gradient, 6031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CRSMatrix* jacobian) { 6041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (cost == NULL && 6051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals == NULL && 6061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling gradient == NULL && 6071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian == NULL) { 6081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(INFO) << "Nothing to do."; 6091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 6101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If the user supplied residual blocks, then use them, otherwise 6131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // take the residual blocks from the underlying program. 6141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Program program; 6151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *program.mutable_residual_blocks() = 6161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ((evaluate_options.residual_blocks.size() > 0) 6171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ? evaluate_options.residual_blocks : program_->residual_blocks()); 6181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<double*>& parameter_block_ptrs = 6201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.parameter_blocks; 6211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ParameterBlock*> variable_parameter_blocks; 6231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ParameterBlock*>& parameter_blocks = 6241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *program.mutable_parameter_blocks(); 6251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (parameter_block_ptrs.size() == 0) { 6271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The user did not provide any parameter blocks, so default to 6281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // using all the parameter blocks in the order that they are in 6291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the underlying program object. 6301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks = program_->parameter_blocks(); 6311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 6321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The user supplied a vector of parameter blocks. Using this list 6331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // requires a number of steps. 6341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 1. Convert double* into ParameterBlock* 6361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks.resize(parameter_block_ptrs.size()); 6371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < parameter_block_ptrs.size(); ++i) { 6381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[i] = 6391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling FindParameterBlockOrDie(parameter_block_map_, 6401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_ptrs[i]); 6411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 2. The user may have only supplied a subset of parameter 6441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // blocks, so identify the ones that are not supplied by the user 6451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // and are NOT constant. These parameter blocks are stored in 6461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // variable_parameter_blocks. 6471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 6481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // To ensure that the parameter blocks are not included in the 6491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // columns of the jacobian, we need to make sure that they are 6501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // constant during evaluation and then make them variable again 6511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // after we are done. 6521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks()); 6531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ParameterBlock*> included_parameter_blocks( 6541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling program.parameter_blocks()); 6551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<ParameterBlock*> excluded_parameter_blocks; 6571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling sort(all_parameter_blocks.begin(), all_parameter_blocks.end()); 6581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling sort(included_parameter_blocks.begin(), included_parameter_blocks.end()); 6591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling set_difference(all_parameter_blocks.begin(), 6601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling all_parameter_blocks.end(), 6611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling included_parameter_blocks.begin(), 6621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling included_parameter_blocks.end(), 6631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling back_inserter(excluded_parameter_blocks)); 6641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling variable_parameter_blocks.reserve(excluded_parameter_blocks.size()); 6661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < excluded_parameter_blocks.size(); ++i) { 6671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ParameterBlock* parameter_block = excluded_parameter_blocks[i]; 6681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!parameter_block->IsConstant()) { 6691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling variable_parameter_blocks.push_back(parameter_block); 6701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block->SetConstant(); 6711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 6741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Setup the Parameter indices and offsets before an evaluator can 6761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // be constructed and used. 6771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling program.SetParameterOffsetsAndIndex(); 6781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Evaluator::Options evaluator_options; 6801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or 6821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // CXSparse, here it just being used for telling the evaluator to 6831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // use a SparseRowCompressedMatrix for the jacobian. This is because 6841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the Evaluator decides the storage for the Jacobian based on the 6851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // type of linear solver being used. 6861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; 6871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluator_options.num_threads = evaluate_options.num_threads; 6881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string error; 6901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<Evaluator> evaluator( 6911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Evaluator::Create(evaluator_options, &program, &error)); 6921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (evaluator.get() == NULL) { 6931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Unable to create an Evaluator object. " 6941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Error: " << error 6951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "This is a Ceres bug; please contact the developers!"; 6961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 6971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Make the parameter blocks that were temporarily marked 6981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // constant, variable again. 6991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < variable_parameter_blocks.size(); ++i) { 7001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling variable_parameter_blocks[i]->SetVarying(); 7011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 70279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 70379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program_->SetParameterBlockStatePtrsToUserStatePtrs(); 70479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program_->SetParameterOffsetsAndIndex(); 7051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return false; 7061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (residuals !=NULL) { 7091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals->resize(evaluator->NumResiduals()); 7101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (gradient != NULL) { 7131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling gradient->resize(evaluator->NumEffectiveParameters()); 7141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian; 7171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobian != NULL) { 7181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling tmp_jacobian.reset( 7191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); 7201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Point the state pointers to the user state pointers. This is 7231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // needed so that we can extract a parameter vector which is then 7241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // passed to Evaluator::Evaluate. 7251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling program.SetParameterBlockStatePtrsToUserStatePtrs(); 7261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Copy the value of the parameter blocks into a vector, since the 7281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Evaluate::Evaluate method needs its input as such. The previous 7291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that 7301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // these values are the ones corresponding to the actual state of 7311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the parameter blocks, rather than the temporary state pointer 7321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // used for evaluation. 7331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector parameters(program.NumParameters()); 7341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling program.ParameterBlocksToStateVector(parameters.data()); 7351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double tmp_cost = 0; 7371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Evaluator::EvaluateOptions evaluator_evaluate_options; 7391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluator_evaluate_options.apply_loss_function = 7401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling evaluate_options.apply_loss_function; 7411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool status = evaluator->Evaluate(evaluator_evaluate_options, 7421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters.data(), 7431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &tmp_cost, 7441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals != NULL ? &(*residuals)[0] : NULL, 7451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling gradient != NULL ? &(*gradient)[0] : NULL, 7461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling tmp_jacobian.get()); 7471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Make the parameter blocks that were temporarily marked constant, 7491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // variable again. 7501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < variable_parameter_blocks.size(); ++i) { 7511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling variable_parameter_blocks[i]->SetVarying(); 7521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (status) { 7551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (cost != NULL) { 7561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *cost = tmp_cost; 7571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobian != NULL) { 7591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling tmp_jacobian->ToCRSMatrix(jacobian); 7601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 7621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 76379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program_->SetParameterBlockStatePtrsToUserStatePtrs(); 76479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program_->SetParameterOffsetsAndIndex(); 7651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return status; 7661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 7671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint ProblemImpl::NumParameterBlocks() const { 7690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return program_->NumParameterBlocks(); 7700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint ProblemImpl::NumParameters() const { 7730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return program_->NumParameters(); 7740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint ProblemImpl::NumResidualBlocks() const { 7770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return program_->NumResidualBlocks(); 7780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint ProblemImpl::NumResiduals() const { 7810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return program_->NumResiduals(); 7820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 7830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 7841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint ProblemImpl::ParameterBlockSize(const double* parameter_block) const { 7851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return FindParameterBlockOrDie(parameter_block_map_, 7861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const_cast<double*>(parameter_block))->Size(); 7871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 7881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 7891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const { 7901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return FindParameterBlockOrDie( 7911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize(); 7921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 7931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 79479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool ProblemImpl::HasParameterBlock(const double* parameter_block) const { 79579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return (parameter_block_map_.find(const_cast<double*>(parameter_block)) != 79679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_block_map_.end()); 79779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 79879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { 8001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_NOTNULL(parameter_blocks); 8011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks->resize(0); 8021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (ParameterMap::const_iterator it = parameter_block_map_.begin(); 8031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling it != parameter_block_map_.end(); 8041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++it) { 8051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks->push_back(it->first); 8061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 8071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 8081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 80979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ProblemImpl::GetResidualBlocks( 81079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<ResidualBlockId>* residual_blocks) const { 81179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(residual_blocks); 81279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *residual_blocks = program().residual_blocks(); 81379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 81479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 81579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ProblemImpl::GetParameterBlocksForResidualBlock( 81679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ResidualBlockId residual_block, 81779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<double*>* parameter_blocks) const { 81879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_parameter_blocks = residual_block->NumParameterBlocks(); 81979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks); 82079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_parameter_blocks; ++i) { 82179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*parameter_blocks)[i] = 82279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block->parameter_blocks()[i]->mutable_user_state(); 82379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 82479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 82579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 82679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid ProblemImpl::GetResidualBlocksForParameterBlock( 82779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double* values, 82879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<ResidualBlockId>* residual_blocks) const { 82979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlock* parameter_block = 83079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez FindParameterBlockOrDie(parameter_block_map_, 83179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const_cast<double*>(values)); 83279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 83379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.enable_fast_removal) { 83479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // In this case the residual blocks that depend on the parameter block are 83579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // stored in the parameter block already, so just copy them out. 83679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(residual_blocks)->resize( 83779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_block->mutable_residual_blocks()->size()); 83879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::copy(parameter_block->mutable_residual_blocks()->begin(), 83979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_block->mutable_residual_blocks()->end(), 84079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_blocks->begin()); 84179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return; 84279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 84379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 84479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Find residual blocks that depend on the parameter block. 84579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(residual_blocks)->clear(); 84679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_residual_blocks = NumResidualBlocks(); 84779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_residual_blocks; ++i) { 84879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* residual_block = 84979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*(program_->mutable_residual_blocks()))[i]; 85079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_parameter_blocks = residual_block->NumParameterBlocks(); 85179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < num_parameter_blocks; ++j) { 85279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (residual_block->parameter_blocks()[j] == parameter_block) { 85379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_blocks->push_back(residual_block); 85479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The parameter blocks are guaranteed unique. 85579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez break; 85679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 85779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 85879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 85979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 8601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 8610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 8620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 863