10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 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: keir@google.com (Keir Mierle) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/program.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <map> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 3579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/array_utils.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/casts.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/compressed_row_sparse_matrix.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/cost_function.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/evaluator.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/local_parameterization.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/loss_function.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/map_util.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/parameter_block.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/problem.h" 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/residual_block.h" 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/stl_util.h" 4879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/triplet_sparse_matrix.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongProgram::Program() {} 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongProgram::Program(const Program& program) 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : parameter_blocks_(program.parameter_blocks_), 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_blocks_(program.residual_blocks_) { 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst vector<ParameterBlock*>& Program::parameter_blocks() const { 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return parameter_blocks_; 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongconst vector<ResidualBlock*>& Program::residual_blocks() const { 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return residual_blocks_; 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvector<ParameterBlock*>* Program::mutable_parameter_blocks() { 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return ¶meter_blocks_; 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvector<ResidualBlock*>* Program::mutable_residual_blocks() { 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return &residual_blocks_; 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool Program::StateVectorToParameterBlocks(const double *state) { 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!parameter_blocks_[i]->IsConstant() && 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong !parameter_blocks_[i]->SetState(state)) { 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong state += parameter_blocks_[i]->Size(); 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid Program::ParameterBlocksToStateVector(double *state) const { 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_blocks_[i]->GetState(state); 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong state += parameter_blocks_[i]->Size(); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid Program::CopyParameterBlockStateToUserState() { 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state()); 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool Program::SetParameterBlockStatePtrsToUserStatePtrs() { 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!parameter_blocks_[i]->IsConstant() && 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) { 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool Program::Plus(const double* state, 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* delta, 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* state_plus_delta) const { 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong state += parameter_blocks_[i]->Size(); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delta += parameter_blocks_[i]->LocalSize(); 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong state_plus_delta += parameter_blocks_[i]->Size(); 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid Program::SetParameterOffsetsAndIndex() { 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Set positions for all parameters appearing as arguments to residuals to one 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // past the end of the parameter block array. 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks_.size(); ++i) { 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ResidualBlock* residual_block = residual_blocks_[i]; 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_block->parameter_blocks()[j]->set_index(-1); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For parameters that appear in the program, set their position and offset. 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int state_offset = 0; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int delta_offset = 0; 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_blocks_[i]->set_index(i); 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_blocks_[i]->set_state_offset(state_offset); 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong parameter_blocks_[i]->set_delta_offset(delta_offset); 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong state_offset += parameter_blocks_[i]->Size(); 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delta_offset += parameter_blocks_[i]->LocalSize(); 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Program::IsValid() const { 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < residual_blocks_.size(); ++i) { 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ResidualBlock* residual_block = residual_blocks_[i]; 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (residual_block->index() != i) { 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(WARNING) << "Residual block: " << i 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << " has incorrect index: " << residual_block->index(); 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int state_offset = 0; 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int delta_offset = 0; 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks_.size(); ++i) { 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ParameterBlock* parameter_block = parameter_blocks_[i]; 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (parameter_block->index() != i || 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_block->state_offset() != state_offset || 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_block->delta_offset() != delta_offset) { 16279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(WARNING) << "Parameter block: " << i 16379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "has incorrect indexing information: " 16479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << parameter_block->ToString(); 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 16879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez state_offset += parameter_blocks_[i]->Size(); 16979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez delta_offset += parameter_blocks_[i]->LocalSize(); 17079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 17179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 17279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 17379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 17479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 17579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Program::ParameterBlocksAreFinite(string* message) const { 17679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(message); 17779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks_.size(); ++i) { 17879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ParameterBlock* parameter_block = parameter_blocks_[i]; 17979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double* array = parameter_block->user_state(); 18079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int size = parameter_block->Size(); 18179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int invalid_index = FindInvalidValue(size, array); 18279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (invalid_index != size) { 18379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = StringPrintf( 18479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "ParameterBlock: %p with size %d has at least one invalid value.\n" 18579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "First invalid value is at index: %d.\n" 18679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Parameter block values: ", 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez array, size, invalid_index); 18879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez AppendArrayToString(size, array, message); 18979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 19079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 19179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 19279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 19379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 19479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 19579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Program::IsBoundsConstrained() const { 19679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks_.size(); ++i) { 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ParameterBlock* parameter_block = parameter_blocks_[i]; 19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (parameter_block->IsConstant()) { 19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez continue; 20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int size = parameter_block->Size(); 20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < size; ++j) { 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double lower_bound = parameter_block->LowerBoundForParameter(j); 20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double upper_bound = parameter_block->UpperBoundForParameter(j); 20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (lower_bound > -std::numeric_limits<double>::max() || 20679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez upper_bound < std::numeric_limits<double>::max()) { 20779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 20879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 20979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 21079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 21179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 21279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 21379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 21479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Program::IsFeasible(string* message) const { 21579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(message); 21679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks_.size(); ++i) { 21779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ParameterBlock* parameter_block = parameter_blocks_[i]; 21879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double* parameters = parameter_block->user_state(); 21979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int size = parameter_block->Size(); 22079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (parameter_block->IsConstant()) { 22179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Constant parameter blocks must start in the feasible region 22279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // to ultimately produce a feasible solution, since Ceres cannot 22379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // change them. 22479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < size; ++j) { 22579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double lower_bound = parameter_block->LowerBoundForParameter(j); 22679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double upper_bound = parameter_block->UpperBoundForParameter(j); 22779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (parameters[j] < lower_bound || parameters[j] > upper_bound) { 22879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = StringPrintf( 22979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "ParameterBlock: %p with size %d has at least one infeasible " 23079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "value." 23179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "\nFirst infeasible value is at index: %d." 23279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "\nLower bound: %e, value: %e, upper bound: %e" 23379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "\nParameter block values: ", 23479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameters, size, j, lower_bound, parameters[j], upper_bound); 23579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez AppendArrayToString(size, parameters, message); 23679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 23779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 23879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 23979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else { 24079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Variable parameter blocks must have non-empty feasible 24179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // regions, otherwise there is no way to produce a feasible 24279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // solution. 24379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < size; ++j) { 24479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double lower_bound = parameter_block->LowerBoundForParameter(j); 24579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double upper_bound = parameter_block->UpperBoundForParameter(j); 24679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (lower_bound >= upper_bound) { 24779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = StringPrintf( 24879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "ParameterBlock: %p with size %d has at least one infeasible " 24979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "bound." 25079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "\nFirst infeasible bound is at index: %d." 25179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "\nLower bound: %e, upper bound: %e" 25279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "\nParameter block values: ", 25379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameters, size, j, lower_bound, upper_bound); 25479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez AppendArrayToString(size, parameters, message); 25579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 25679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 25779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 25879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 25979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 26079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 26179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 26279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 26379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 26479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezProgram* Program::CreateReducedProgram(vector<double*>* removed_parameter_blocks, 26579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* fixed_cost, 26679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* error) const { 26779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(removed_parameter_blocks); 26879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(fixed_cost); 26979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(error); 27079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 27179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez scoped_ptr<Program> reduced_program(new Program(*this)); 27279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks, 27379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez fixed_cost, 27479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez error)) { 27579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return NULL; 27679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 27779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 27879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez reduced_program->SetParameterOffsetsAndIndex(); 27979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return reduced_program.release(); 28079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 28179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 28279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks, 28379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* fixed_cost, 28479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* error) { 28579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(removed_parameter_blocks); 28679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(fixed_cost); 28779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(error); 28879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 28979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez scoped_array<double> residual_block_evaluate_scratch; 29079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block_evaluate_scratch.reset( 29179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez new double[MaxScratchDoublesNeededForEvaluate()]); 29279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *fixed_cost = 0.0; 29379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 29479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Mark all the parameters as unused. Abuse the index member of the 29579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // parameter blocks for the marking. 29679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks_.size(); ++i) { 29779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks_[i]->set_index(-1); 29879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 29979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Filter out residual that have all-constant parameters, and mark 30179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // all the parameter blocks that appear in residuals. 30279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_active_residual_blocks = 0; 30379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < residual_blocks_.size(); ++i) { 30479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ResidualBlock* residual_block = residual_blocks_[i]; 30579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_parameter_blocks = residual_block->NumParameterBlocks(); 30679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Determine if the residual block is fixed, and also mark varying 30879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // parameters that appear in the residual block. 30979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool all_constant = true; 31079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int k = 0; k < num_parameter_blocks; k++) { 31179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; 31279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!parameter_block->IsConstant()) { 31379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez all_constant = false; 31479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_block->set_index(1); 31579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 31679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 31779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 31879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!all_constant) { 31979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_blocks_[num_active_residual_blocks++] = residual_block; 32079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez continue; 32179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 32279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 32379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The residual is constant and will be removed, so its cost is 32479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // added to the variable fixed_cost. 32579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double cost = 0.0; 32679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!residual_block->Evaluate(true, 32779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &cost, 32879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez NULL, 32979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez NULL, 33079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block_evaluate_scratch.get())) { 33179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *error = StringPrintf("Evaluation of the residual %d failed during " 33279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "removal of fixed residual blocks.", i); 33379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 33479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 33579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *fixed_cost += cost; 33679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 33779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_blocks_.resize(num_active_residual_blocks); 33879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 33979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Filter out unused or fixed parameter blocks. 34079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_active_parameter_blocks = 0; 34179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez removed_parameter_blocks->clear(); 34279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < parameter_blocks_.size(); ++i) { 34379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlock* parameter_block = parameter_blocks_[i]; 34479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (parameter_block->index() == -1) { 34579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez removed_parameter_blocks->push_back(parameter_block->mutable_user_state()); 34679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else { 34779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks_[num_active_parameter_blocks++] = parameter_block; 34879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 34979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 35079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks_.resize(num_active_parameter_blocks); 35179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!(((NumResidualBlocks() == 0) && 35379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (NumParameterBlocks() == 0)) || 35479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ((NumResidualBlocks() != 0) && 35579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (NumParameterBlocks() != 0)))) { 35679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *error = "Congratulations, you found a bug in Ceres. Please report it."; 35779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 35879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 35979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 36179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 36279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Program::IsParameterBlockSetIndependent(const set<double*>& independent_set) const { 36479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Loop over each residual block and ensure that no two parameter 36579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // blocks in the same residual block are part of 36679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // parameter_block_ptrs as that would violate the assumption that it 36779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // is an independent set in the Hessian matrix. 36879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (vector<ResidualBlock*>::const_iterator it = residual_blocks_.begin(); 36979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez it != residual_blocks_.end(); 37079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++it) { 37179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); 37279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_parameter_blocks = (*it)->NumParameterBlocks(); 37379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int count = 0; 37479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_parameter_blocks; ++i) { 37579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez count += independent_set.count( 37679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks[i]->mutable_user_state()); 37779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 37879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (count > 1) { 37979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 38079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 38179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 38279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 38379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 38479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 38579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezTripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const { 38679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Matrix to store the block sparsity structure of the Jacobian. 38779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez TripletSparseMatrix* tsm = 38879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez new TripletSparseMatrix(NumParameterBlocks(), 38979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez NumResidualBlocks(), 39079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10 * NumResidualBlocks()); 39179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_nonzeros = 0; 39279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int* rows = tsm->mutable_rows(); 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int* cols = tsm->mutable_cols(); 39479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* values = tsm->mutable_values(); 39579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 39679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int c = 0; c < residual_blocks_.size(); ++c) { 39779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ResidualBlock* residual_block = residual_blocks_[c]; 39879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_parameter_blocks = residual_block->NumParameterBlocks(); 39979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ParameterBlock* const* parameter_blocks = 40079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residual_block->parameter_blocks(); 40179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int j = 0; j < num_parameter_blocks; ++j) { 40379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (parameter_blocks[j]->IsConstant()) { 40479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez continue; 40579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Re-size the matrix if needed. 40879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (num_nonzeros >= tsm->max_num_nonzeros()) { 40979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez tsm->set_num_nonzeros(num_nonzeros); 41079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez tsm->Reserve(2 * num_nonzeros); 41179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez rows = tsm->mutable_rows(); 41279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cols = tsm->mutable_cols(); 41379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez values = tsm->mutable_values(); 41479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 41579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int r = parameter_blocks[j]->index(); 41779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez rows[num_nonzeros] = r; 41879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cols[num_nonzeros] = c; 41979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez values[num_nonzeros] = 1.0; 42079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++num_nonzeros; 42179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 42279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 42379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 42479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez tsm->set_num_nonzeros(num_nonzeros); 42579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return tsm; 42679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 42779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::NumResidualBlocks() const { 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return residual_blocks_.size(); 4300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::NumParameterBlocks() const { 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return parameter_blocks_.size(); 4340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::NumResiduals() const { 4370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_residuals = 0; 4380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks_.size(); ++i) { 4390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_residuals += residual_blocks_[i]->NumResiduals(); 4400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return num_residuals; 4420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::NumParameters() const { 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_parameters = 0; 4460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 4470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_parameters += parameter_blocks_[i]->Size(); 4480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return num_parameters; 4500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::NumEffectiveParameters() const { 4530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_parameters = 0; 4540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 4550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_parameters += parameter_blocks_[i]->LocalSize(); 4560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return num_parameters; 4580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::MaxScratchDoublesNeededForEvaluate() const { 4610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute the scratch space needed for evaluate. 4620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_scratch_bytes_for_evaluate = 0; 4630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks_.size(); ++i) { 4640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_scratch_bytes_for_evaluate = 4650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max(max_scratch_bytes_for_evaluate, 4660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_blocks_[i]->NumScratchDoublesForEvaluate()); 4670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return max_scratch_bytes_for_evaluate; 4690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::MaxDerivativesPerResidualBlock() const { 4720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_derivatives = 0; 4730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks_.size(); ++i) { 4740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int derivatives = 0; 4750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ResidualBlock* residual_block = residual_blocks_[i]; 4760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_parameters = residual_block->NumParameterBlocks(); 4770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < num_parameters; ++j) { 4780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong derivatives += residual_block->NumResiduals() * 4790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_block->parameter_blocks()[j]->LocalSize(); 4800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_derivatives = max(max_derivatives, derivatives); 4820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return max_derivatives; 4840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::MaxParametersPerResidualBlock() const { 4870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_parameters = 0; 4880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks_.size(); ++i) { 4890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_parameters = max(max_parameters, 4900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_blocks_[i]->NumParameterBlocks()); 4910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return max_parameters; 4930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongint Program::MaxResidualsPerResidualBlock() const { 4960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_residuals = 0; 4970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < residual_blocks_.size(); ++i) { 4980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong max_residuals = max(max_residuals, 4990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong residual_blocks_[i]->NumResiduals()); 5000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return max_residuals; 5020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstring Program::ToString() const { 5050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong string ret = "Program dump\n"; 5060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks()); 5070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ret += StringPrintf("Number of parameters: %d\n", NumParameters()); 5080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ret += "Parameters:\n"; 5090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < parameter_blocks_.size(); ++i) { 5100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ret += StringPrintf("%d: %s\n", 5110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong i, parameter_blocks_[i]->ToString().c_str()); 5120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return ret; 5140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 518