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@google.com (Sameer Agarwal)
30//         mierle@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
47namespace ceres {
48namespace internal {
49
50// Helper templates that allow evaluation of a variadic functor or a
51// CostFunction object.
52template <typename CostFunctor,
53          int N0, int N1, int N2, int N3, int N4,
54          int N5, int N6, int N7, int N8, int N9 >
55bool 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
67template <typename CostFunctor,
68          int N0, int N1, int N2, int N3, int N4,
69          int N5, int N6, int N7, int N8, int N9 >
70bool 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.
80template <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>
87struct 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
175template <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>
181struct 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