1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 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: sameeragarwal (at) google.com (Sameer Agarwal) 30 // mierle (at) gmail.com (Keir Mierle) 31 // 32 // Finite differencing routine used by NumericDiffCostFunction. 33 34 #ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ 35 #define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ 36 37 #include <cstring> 38 39 #include "Eigen/Dense" 40 #include "ceres/cost_function.h" 41 #include "ceres/internal/scoped_ptr.h" 42 #include "ceres/internal/variadic_evaluate.h" 43 #include "ceres/types.h" 44 #include "glog/logging.h" 45 46 47 namespace ceres { 48 namespace internal { 49 50 // Helper templates that allow evaluation of a variadic functor or a 51 // CostFunction object. 52 template <typename CostFunctor, 53 int N0, int N1, int N2, int N3, int N4, 54 int N5, int N6, int N7, int N8, int N9 > 55 bool EvaluateImpl(const CostFunctor* functor, 56 double const* const* parameters, 57 double* residuals, 58 const void* /* NOT USED */) { 59 return VariadicEvaluate<CostFunctor, 60 double, 61 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( 62 *functor, 63 parameters, 64 residuals); 65 } 66 67 template <typename CostFunctor, 68 int N0, int N1, int N2, int N3, int N4, 69 int N5, int N6, int N7, int N8, int N9 > 70 bool EvaluateImpl(const CostFunctor* functor, 71 double const* const* parameters, 72 double* residuals, 73 const CostFunction* /* NOT USED */) { 74 return functor->Evaluate(parameters, residuals, NULL); 75 } 76 77 // This is split from the main class because C++ doesn't allow partial template 78 // specializations for member functions. The alternative is to repeat the main 79 // class for differing numbers of parameters, which is also unfortunate. 80 template <typename CostFunctor, 81 NumericDiffMethod kMethod, 82 int kNumResiduals, 83 int N0, int N1, int N2, int N3, int N4, 84 int N5, int N6, int N7, int N8, int N9, 85 int kParameterBlock, 86 int kParameterBlockSize> 87 struct NumericDiff { 88 // Mutates parameters but must restore them before return. 89 static bool EvaluateJacobianForParameterBlock( 90 const CostFunctor* functor, 91 double const* residuals_at_eval_point, 92 const double relative_step_size, 93 double **parameters, 94 double *jacobian) { 95 using Eigen::Map; 96 using Eigen::Matrix; 97 using Eigen::RowMajor; 98 using Eigen::ColMajor; 99 100 typedef Matrix<double, kNumResiduals, 1> ResidualVector; 101 typedef Matrix<double, kParameterBlockSize, 1> ParameterVector; 102 typedef Matrix<double, kNumResiduals, kParameterBlockSize, 103 (kParameterBlockSize == 1 && 104 kNumResiduals > 1) ? ColMajor : RowMajor> JacobianMatrix; 105 106 107 Map<JacobianMatrix> parameter_jacobian(jacobian, 108 kNumResiduals, 109 kParameterBlockSize); 110 111 // Mutate 1 element at a time and then restore. 112 Map<ParameterVector> x_plus_delta(parameters[kParameterBlock], 113 kParameterBlockSize); 114 ParameterVector x(x_plus_delta); 115 ParameterVector step_size = x.array().abs() * relative_step_size; 116 117 // To handle cases where a parameter is exactly zero, instead use 118 // the mean step_size for the other dimensions. If all the 119 // parameters are zero, there's no good answer. Take 120 // relative_step_size as a guess and hope for the best. 121 const double fallback_step_size = 122 (step_size.sum() == 0) 123 ? relative_step_size 124 : step_size.sum() / step_size.rows(); 125 126 // For each parameter in the parameter block, use finite differences to 127 // compute the derivative for that parameter. 128 for (int j = 0; j < kParameterBlockSize; ++j) { 129 const double delta = 130 (step_size(j) == 0.0) ? fallback_step_size : step_size(j); 131 132 x_plus_delta(j) = x(j) + delta; 133 134 double residuals[kNumResiduals]; // NOLINT 135 136 if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( 137 functor, parameters, residuals, functor)) { 138 return false; 139 } 140 141 // Compute this column of the jacobian in 3 steps: 142 // 1. Store residuals for the forward part. 143 // 2. Subtract residuals for the backward (or 0) part. 144 // 3. Divide out the run. 145 parameter_jacobian.col(j) = 146 Map<const ResidualVector>(residuals, kNumResiduals); 147 148 double one_over_delta = 1.0 / delta; 149 if (kMethod == CENTRAL) { 150 // Compute the function on the other side of x(j). 151 x_plus_delta(j) = x(j) - delta; 152 153 if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>( 154 functor, parameters, residuals, functor)) { 155 return false; 156 } 157 158 parameter_jacobian.col(j) -= 159 Map<ResidualVector>(residuals, kNumResiduals, 1); 160 one_over_delta /= 2; 161 } else { 162 // Forward difference only; reuse existing residuals evaluation. 163 parameter_jacobian.col(j) -= 164 Map<const ResidualVector>(residuals_at_eval_point, kNumResiduals); 165 } 166 x_plus_delta(j) = x(j); // Restore x_plus_delta. 167 168 // Divide out the run to get slope. 169 parameter_jacobian.col(j) *= one_over_delta; 170 } 171 return true; 172 } 173 }; 174 175 template <typename CostFunctor, 176 NumericDiffMethod kMethod, 177 int kNumResiduals, 178 int N0, int N1, int N2, int N3, int N4, 179 int N5, int N6, int N7, int N8, int N9, 180 int kParameterBlock> 181 struct NumericDiff<CostFunctor, kMethod, kNumResiduals, 182 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9, 183 kParameterBlock, 0> { 184 // Mutates parameters but must restore them before return. 185 static bool EvaluateJacobianForParameterBlock( 186 const CostFunctor* functor, 187 double const* residuals_at_eval_point, 188 const double relative_step_size, 189 double **parameters, 190 double *jacobian) { 191 LOG(FATAL) << "Control should never reach here."; 192 return true; 193 } 194 }; 195 196 } // namespace internal 197 } // namespace ceres 198 199 #endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ 200
