1d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// Ceres Solver - A fast non-linear least squares minimizer 2d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// Copyright 2012 Google Inc. All rights reserved. 3d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// http://code.google.com/p/ceres-solver/ 4d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// 5d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// Redistribution and use in source and binary forms, with or without 6d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// modification, are permitted provided that the following conditions are met: 7d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// 8d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// * Redistributions of source code must retain the above copyright notice, 9d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// this list of conditions and the following disclaimer. 10d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// * Redistributions in binary form must reproduce the above copyright notice, 11d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// this list of conditions and the following disclaimer in the documentation 12d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// and/or other materials provided with the distribution. 13d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// * Neither the name of Google Inc. nor the names of its contributors may be 14ada3a6017e603965f87fa34f6e2fa60379e8d697machenbach@chromium.org// used to endorse or promote products derived from this software without 15d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// specific prior written permission. 16d4f11c0cf476dd854eaebec1cbacb1afc7bea18emachenbach@chromium.org// 179aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 189aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 199aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 209aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 216313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 229aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 239aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 249aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 259aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 269aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 279aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// POSSIBILITY OF SUCH DAMAGE. 289aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// 299aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// Author: mierle@gmail.com (Keir Mierle) 309aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// sameeragarwal@google.com (Sameer Agarwal) 319aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// thadh@gmail.com (Thad Hughes) 326313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// 336313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// This numeric diff implementation differs from the one found in 349aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// numeric_diff_cost_function.h by supporting numericdiff on cost 359aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// functions with variable numbers of parameters with variable 369aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// sizes. With the other implementation, all the sizes (both the 376313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// number of parameter blocks and the size of each block) must be 386313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// fixed at compile time. 399aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// 409aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// The functor API differs slightly from the API for fixed size 416313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// numeric diff; the expected interface for the cost functors is: 426313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// 439aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// struct MyCostFunctor { 449aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// template<typename T> 456313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// bool operator()(double const* const* parameters, double* residuals) const { 469aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// // Use parameters[i] to access the i'th parameter block. 479aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// } 486313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// } 496313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// 506313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// Since the sizing of the parameters is done at runtime, you must 516313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// also specify the sizes after creating the 526313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// DynamicNumericDiffCostFunction. For example: 536313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org// 549aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function( 559aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// new MyCostFunctor()); 569aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// cost_function.AddParameterBlock(5); 579aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// cost_function.AddParameterBlock(10); 589aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org// cost_function.SetNumResiduals(21); 599aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 609aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ 619aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ 629aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 639aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include <cmath> 649aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include <numeric> 659aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include <vector> 669aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 679aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include "ceres/cost_function.h" 689aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include "ceres/internal/scoped_ptr.h" 699aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include "ceres/internal/eigen.h" 709aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include "ceres/internal/numeric_diff.h" 719aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#include "glog/logging.h" 729aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 739aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.orgnamespace ceres { 749aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 759aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.orgtemplate <typename CostFunctor, NumericDiffMethod method = CENTRAL> 769aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.orgclass DynamicNumericDiffCostFunction : public CostFunction { 779aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org public: 789aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org explicit DynamicNumericDiffCostFunction(const CostFunctor* functor, 799aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org Ownership ownership = TAKE_OWNERSHIP, 809aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double relative_step_size = 1e-6) 819aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org : functor_(functor), 829aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org ownership_(ownership), 839aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org relative_step_size_(relative_step_size) { 849aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 859aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 869aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org virtual ~DynamicNumericDiffCostFunction() { 879aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (ownership_ != TAKE_OWNERSHIP) { 889aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org functor_.release(); 899aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 909aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 919aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 929aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org void AddParameterBlock(int size) { 939aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org mutable_parameter_block_sizes()->push_back(size); 949aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 959aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 969aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org void SetNumResiduals(int num_residuals) { 979aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org set_num_residuals(num_residuals); 989aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 999aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1009aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org virtual bool Evaluate(double const* const* parameters, 1019aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double* residuals, 1029aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double** jacobians) const { 1039aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org CHECK_GT(num_residuals(), 0) 1049aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() " 1056313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org << "before DynamicNumericDiffCostFunction::Evaluate()."; 1066313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org 1076313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org const vector<int32>& block_sizes = parameter_block_sizes(); 1086313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org CHECK(!block_sizes.empty()) 1096313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " 1106313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org << "before DynamicNumericDiffCostFunction::Evaluate()."; 1116313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org 1129aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org const bool status = EvaluateCostFunctor(parameters, residuals); 1139aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (jacobians == NULL || !status) { 1149aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return status; 1159aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1169aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1179aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Create local space for a copy of the parameters which will get mutated. 1186313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); 1196313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org vector<double> parameters_copy(parameters_size); 1206313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org vector<double*> parameters_references_copy(block_sizes.size()); 1216313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org parameters_references_copy[0] = ¶meters_copy[0]; 1226313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org for (int block = 1; block < block_sizes.size(); ++block) { 1236313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org parameters_references_copy[block] = parameters_references_copy[block - 1] 1246313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org + block_sizes[block - 1]; 1256313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org } 1266313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org 1276313e220249748eb26e1ddcee2bbe857fef03b42machenbach@chromium.org // Copy the parameters into the local temp space. 1289aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org for (int block = 0; block < block_sizes.size(); ++block) { 1299aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org memcpy(parameters_references_copy[block], 1309aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameters[block], 1319aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org block_sizes[block] * sizeof(*parameters[block])); 1329aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1339aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1349aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org for (int block = 0; block < block_sizes.size(); ++block) { 1359aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (jacobians[block] != NULL && 1369aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org !EvaluateJacobianForParameterBlock(block_sizes[block], 1379aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org block, 1389aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org relative_step_size_, 1399aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org residuals, 1409aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org ¶meters_references_copy[0], 1419aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org jacobians)) { 1429aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return false; 1439aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1449aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1459aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return true; 1469aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1479aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1489aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org private: 1499aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org bool EvaluateJacobianForParameterBlock(const int parameter_block_size, 1509aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org const int parameter_block, 1519aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org const double relative_step_size, 1529aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double const* residuals_at_eval_point, 1539aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double** parameters, 1549aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double** jacobians) const { 1559aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org using Eigen::Map; 1569aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org using Eigen::Matrix; 1579aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org using Eigen::Dynamic; 1589aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org using Eigen::RowMajor; 1599aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1609aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org typedef Matrix<double, Dynamic, 1> ResidualVector; 1619aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org typedef Matrix<double, Dynamic, 1> ParameterVector; 1629aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix; 1639aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1649aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org int num_residuals = this->num_residuals(); 1659aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1669aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], 1679aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org num_residuals, 1689aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameter_block_size); 1699aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1709aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Mutate one element at a time and then restore. 1719aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org Map<ParameterVector> x_plus_delta(parameters[parameter_block], 1729aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameter_block_size); 1739aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org ParameterVector x(x_plus_delta); 1749aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org ParameterVector step_size = x.array().abs() * relative_step_size; 1759aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1769aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // To handle cases where a paremeter is exactly zero, instead use 1779aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // the mean step_size for the other dimensions. 1789aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double fallback_step_size = step_size.sum() / step_size.rows(); 1799aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (fallback_step_size == 0.0) { 1809aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // If all the parameters are zero, there's no good answer. Use the given 1819aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // relative step_size as absolute step_size and hope for the best. 1829aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org fallback_step_size = relative_step_size; 1839aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1849aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1859aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // For each parameter in the parameter block, use finite 1869aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // differences to compute the derivative for that parameter. 1879aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org for (int j = 0; j < parameter_block_size; ++j) { 1889aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (step_size(j) == 0.0) { 1899aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // The parameter is exactly zero, so compromise and use the 1909aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // mean step_size from the other parameters. This can break in 1919aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // many cases, but it's hard to pick a good number without 1929aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // problem specific knowledge. 1939aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org step_size(j) = fallback_step_size; 1949aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 1959aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org x_plus_delta(j) = x(j) + step_size(j); 1969aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 1979aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org ResidualVector residuals(num_residuals); 1989aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (!EvaluateCostFunctor(parameters, &residuals[0])) { 1999aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Something went wrong; bail. 2009aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return false; 2019aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2029aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2039aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Compute this column of the jacobian in 3 steps: 2049aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // 1. Store residuals for the forward part. 2059aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // 2. Subtract residuals for the backward (or 0) part. 2069aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // 3. Divide out the run. 2079aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameter_jacobian.col(j).matrix() = residuals; 2089aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2099aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double one_over_h = 1 / step_size(j); 2109aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (method == CENTRAL) { 2119aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Compute the function on the other side of x(j). 2129aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org x_plus_delta(j) = x(j) - step_size(j); 2139aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2149aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org if (!EvaluateCostFunctor(parameters, &residuals[0])) { 2159aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Something went wrong; bail. 2169aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return false; 2179aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2189aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2199aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameter_jacobian.col(j) -= residuals; 2209aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org one_over_h /= 2; 2219aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } else { 2229aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Forward difference only; reuse existing residuals evaluation. 2239aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameter_jacobian.col(j) -= 2249aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org Map<const ResidualVector>(residuals_at_eval_point, num_residuals); 2259aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2269aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org x_plus_delta(j) = x(j); // Restore x_plus_delta. 2279aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2289aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Divide out the run to get slope. 2299aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameter_jacobian.col(j) *= one_over_h; 2309aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2319aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return true; 2329aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2339aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2349aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org bool EvaluateCostFunctor(double const* const* parameters, 2359aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double* residuals) const { 2369aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return EvaluateCostFunctorImpl(functor_.get(), 2379aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org parameters, 2389aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org residuals, 2399aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org functor_.get()); 2409aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2419aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2429aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // Helper templates to allow evaluation of a functor or a 2439aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org // CostFunction. 2449aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org bool EvaluateCostFunctorImpl(const CostFunctor* functor, 2459aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double const* const* parameters, 2469aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double* residuals, 2479aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org const void* /* NOT USED */) const { 2489aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return (*functor)(parameters, residuals); 2499aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2509aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2519aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org bool EvaluateCostFunctorImpl(const CostFunctor* functor, 2529aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double const* const* parameters, 2539aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org double* residuals, 2549aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org const CostFunction* /* NOT USED */) const { 2559aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org return functor->Evaluate(parameters, residuals, NULL); 2569aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org } 2579aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2589aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org internal::scoped_ptr<const CostFunctor> functor_; 2599aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org Ownership ownership_; 2609aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org const double relative_step_size_; 2619aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org}; 2629aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2639aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org} // namespace ceres 2649aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org 2659aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ 2669aaa825cf89e1bcfece269a453300ebf4a26d64dmachenbach@chromium.org