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: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// keir@google.com (Keir Mierle) 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Purpose : Class and struct definitions for parameter and residual blocks. 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_INTERNAL_RESIDUAL_BLOCK_H_ 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <string> 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/cost_function.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/stringprintf.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass LossFunction; 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass ParameterBlock; 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A term in the least squares problem. The mathematical form of each term in 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the overall least-squares cost function is: 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// --- loss_function( || cost_function(block1, block2, ...) ||^2 ), 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Storing the cost function and the loss function separately permits optimizing 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the problem with standard non-linear least techniques, without requiring a 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// more general non-linear solver. 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The residual block stores pointers to but does not own the cost functions, 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// loss functions, and parameter blocks. 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass ResidualBlock { 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Construct the residual block with the given cost/loss functions. Loss may 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // be null. The index is the index of the residual block in the Program's 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // residual_blocks array. 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ResidualBlock(const CostFunction* cost_function, 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LossFunction* loss_function, 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<ParameterBlock*>& parameter_blocks, 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int index); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Evaluates the residual term, storing the scalar cost in *cost, the residual 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // components in *residuals, and the jacobians between the parameters and 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // residuals in jacobians[i], in row-major order. If residuals is NULL, the 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // residuals are not computed. If jacobians is NULL, no jacobians are 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // computed. If jacobians[i] is NULL, then the jacobian for that parameter is 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // not computed. 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Evaluate needs scratch space which must be supplied by the caller via 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // scratch. The array should have at least NumScratchDoublesForEvaluate() 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // space available. 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The return value indicates the success or failure. If the function returns 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // false, the caller should expect the the output memory locations to have 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // been modified. 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The returned cost and jacobians have had robustification and local 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // parameterizations applied already; for example, the jacobian for a 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 4-dimensional quaternion parameter using the "QuaternionParameterization" 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // is num_residuals by 3 instead of num_residuals by 4. 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // apply_loss_function as the name implies allows the user to switch 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the application of the loss function on and off. 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool Evaluate(bool apply_loss_function, 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* cost, 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* residuals, 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double** jacobians, 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* scratch) const; 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CostFunction* cost_function() const { return cost_function_; } 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LossFunction* loss_function() const { return loss_function_; } 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Access the parameter blocks for this residual. The array has size 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // NumParameterBlocks(). 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ParameterBlock* const* parameter_blocks() const { 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return parameter_blocks_.get(); 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Number of variable blocks that this residual term depends on. 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int NumParameterBlocks() const { 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return cost_function_->parameter_block_sizes().size(); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The size of the residual vector returned by this residual function. 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int NumResiduals() const { return cost_function_->num_residuals(); } 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The minimum amount of scratch space needed to pass to Evaluate(). 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int NumScratchDoublesForEvaluate() const; 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // This residual block's index in an array. 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int index() const { return index_; } 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void set_index(int index) { index_ = index; } 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling string ToString() { 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return StringPrintf("{residual block; index=%d}", index_); 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CostFunction* cost_function_; 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LossFunction* loss_function_; 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scoped_array<ParameterBlock*> parameter_blocks_; 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The index of the residual, typically in a Program. This is only to permit 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // switching from a ResidualBlock* to an index in the Program's array, needed 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // to do efficient removals. 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int32 index_; 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_INTERNAL_RESIDUAL_BLOCK_H_ 149