residual_block.cc revision 0ae28bd5885b5daa526898fcf7c323dc2c3e1963
1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: keir@google.com (Keir Mierle) 30// sameeragarwal@google.com (Sameer Agarwal) 31 32#include "ceres/residual_block.h" 33 34#include <algorithm> 35#include <cstddef> 36#include <vector> 37 38#include "ceres/corrector.h" 39#include "ceres/parameter_block.h" 40#include "ceres/residual_block_utils.h" 41#include "ceres/cost_function.h" 42#include "ceres/internal/eigen.h" 43#include "ceres/internal/fixed_array.h" 44#include "ceres/local_parameterization.h" 45#include "ceres/loss_function.h" 46 47namespace ceres { 48namespace internal { 49 50ResidualBlock::ResidualBlock(const CostFunction* cost_function, 51 const LossFunction* loss_function, 52 const vector<ParameterBlock*>& parameter_blocks) 53 : cost_function_(cost_function), 54 loss_function_(loss_function), 55 parameter_blocks_( 56 new ParameterBlock* [ 57 cost_function->parameter_block_sizes().size()]) { 58 std::copy(parameter_blocks.begin(), 59 parameter_blocks.end(), 60 parameter_blocks_.get()); 61} 62 63bool ResidualBlock::Evaluate(double* cost, 64 double* residuals, 65 double** jacobians, 66 double* scratch) const { 67 const int num_parameter_blocks = NumParameterBlocks(); 68 const int num_residuals = cost_function_->num_residuals(); 69 70 // Collect the parameters from their blocks. This will rarely allocate, since 71 // residuals taking more than 8 parameter block arguments are rare. 72 FixedArray<const double*, 8> parameters(num_parameter_blocks); 73 for (int i = 0; i < num_parameter_blocks; ++i) { 74 parameters[i] = parameter_blocks_[i]->state(); 75 } 76 77 // Put pointers into the scratch space into global_jacobians as appropriate. 78 FixedArray<double*, 8> global_jacobians(num_parameter_blocks); 79 if (jacobians != NULL) { 80 for (int i = 0; i < num_parameter_blocks; ++i) { 81 const ParameterBlock* parameter_block = parameter_blocks_[i]; 82 if (jacobians[i] != NULL && 83 parameter_block->LocalParameterizationJacobian() != NULL) { 84 global_jacobians[i] = scratch; 85 scratch += num_residuals * parameter_block->Size(); 86 } else { 87 global_jacobians[i] = jacobians[i]; 88 } 89 } 90 } 91 92 // If the caller didn't request residuals, use the scratch space for them. 93 bool outputting_residuals = (residuals != NULL); 94 if (!outputting_residuals) { 95 residuals = scratch; 96 } 97 98 // Invalidate the evaluation buffers so that we can check them after 99 // the CostFunction::Evaluate call, to see if all the return values 100 // that were required were written to and that they are finite. 101 double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; 102 103 InvalidateEvaluation(*this, cost, residuals, eval_jacobians); 104 105 if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians)) { 106 return false; 107 } 108 109 if (!IsEvaluationValid(*this, 110 parameters.get(), 111 cost, 112 residuals, 113 eval_jacobians)) { 114 string message = 115 "\n\n" 116 "Error in evaluating the ResidualBlock.\n\n" 117 "There are two possible reasons. Either the CostFunction did not evaluate and fill all \n" // NOLINT 118 "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT 119 "generated during the or jacobian computation. \n\n" + 120 EvaluationToString(*this, 121 parameters.get(), 122 cost, 123 residuals, 124 eval_jacobians); 125 LOG(WARNING) << message; 126 return false; 127 } 128 129 double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); 130 131 // Update the jacobians with the local parameterizations. 132 if (jacobians != NULL) { 133 for (int i = 0; i < num_parameter_blocks; ++i) { 134 if (jacobians[i] != NULL) { 135 const ParameterBlock* parameter_block = parameter_blocks_[i]; 136 137 // Apply local reparameterization to the jacobians. 138 if (parameter_block->LocalParameterizationJacobian() != NULL) { 139 ConstMatrixRef local_to_global( 140 parameter_block->LocalParameterizationJacobian(), 141 parameter_block->Size(), 142 parameter_block->LocalSize()); 143 MatrixRef global_jacobian(global_jacobians[i], 144 num_residuals, 145 parameter_block->Size()); 146 MatrixRef local_jacobian(jacobians[i], 147 num_residuals, 148 parameter_block->LocalSize()); 149 local_jacobian.noalias() = global_jacobian * local_to_global; 150 } 151 } 152 } 153 } 154 155 if (loss_function_ == NULL) { 156 *cost = 0.5 * squared_norm; 157 return true; 158 } 159 160 double rho[3]; 161 loss_function_->Evaluate(squared_norm, rho); 162 *cost = 0.5 * rho[0]; 163 164 // No jacobians and not outputting residuals? All done. Doing an early exit 165 // here avoids constructing the "Corrector" object below in a common case. 166 if (jacobians == NULL && !outputting_residuals) { 167 return true; 168 } 169 170 // Correct for the effects of the loss function. The jacobians need to be 171 // corrected before the residuals, since they use the uncorrected residuals. 172 Corrector correct(squared_norm, rho); 173 if (jacobians != NULL) { 174 for (int i = 0; i < num_parameter_blocks; ++i) { 175 if (jacobians[i] != NULL) { 176 const ParameterBlock* parameter_block = parameter_blocks_[i]; 177 178 // Correct the jacobians for the loss function. 179 correct.CorrectJacobian(num_residuals, 180 parameter_block->LocalSize(), 181 residuals, 182 jacobians[i]); 183 } 184 } 185 } 186 187 // Correct the residuals with the loss function. 188 if (outputting_residuals) { 189 correct.CorrectResiduals(num_residuals, residuals); 190 } 191 return true; 192} 193 194int ResidualBlock::NumScratchDoublesForEvaluate() const { 195 // Compute the amount of scratch space needed to store the full-sized 196 // jacobians. For parameters that have no local parameterization no storage 197 // is needed and the passed-in jacobian array is used directly. Also include 198 // space to store the residuals, which is needed for cost-only evaluations. 199 // This is slightly pessimistic, since both won't be needed all the time, but 200 // the amount of excess should not cause problems for the caller. 201 int num_parameters = NumParameterBlocks(); 202 int scratch_doubles = 1; 203 for (int i = 0; i < num_parameters; ++i) { 204 const ParameterBlock* parameter_block = parameter_blocks_[i]; 205 if (!parameter_block->IsConstant() && 206 parameter_block->LocalParameterizationJacobian() != NULL) { 207 scratch_doubles += parameter_block->Size(); 208 } 209 } 210 scratch_doubles *= NumResiduals(); 211 return scratch_doubles; 212} 213 214} // namespace internal 215} // namespace ceres 216