1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2019 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
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: mierle@gmail.com (Keir Mierle)
30 //         sameeragarwal@google.com (Sameer Agarwal)
31 //         thadh@gmail.com (Thad Hughes)
32 //         tbennun@gmail.com (Tal Ben-Nun)
33 
34 #ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
35 #define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
36 
37 #include <cmath>
38 #include <memory>
39 #include <numeric>
40 #include <vector>
41 
42 #include "ceres/dynamic_cost_function.h"
43 #include "ceres/internal/eigen.h"
44 #include "ceres/internal/numeric_diff.h"
45 #include "ceres/internal/parameter_dims.h"
46 #include "ceres/numeric_diff_options.h"
47 #include "glog/logging.h"
48 
49 namespace ceres {
50 
51 // This numeric diff implementation differs from the one found in
52 // numeric_diff_cost_function.h by supporting numericdiff on cost
53 // functions with variable numbers of parameters with variable
54 // sizes. With the other implementation, all the sizes (both the
55 // number of parameter blocks and the size of each block) must be
56 // fixed at compile time.
57 //
58 // The functor API differs slightly from the API for fixed size
59 // numeric diff; the expected interface for the cost functors is:
60 //
61 //   struct MyCostFunctor {
62 //     bool operator()(double const*
63 //                     const* parameters,
64 //                     double* residuals) const {
65 //       // Use parameters[i] to access the i'th parameter block.
66 //     }
67 //   }
68 //
69 // Since the sizing of the parameters is done at runtime, you must
70 // also specify the sizes after creating the
71 // DynamicNumericDiffCostFunction. For example:
72 //
73 //   DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function(
74 //       new MyCostFunctor());
75 //   cost_function.AddParameterBlock(5);
76 //   cost_function.AddParameterBlock(10);
77 //   cost_function.SetNumResiduals(21);
78 template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
79 class DynamicNumericDiffCostFunction : public DynamicCostFunction {
80  public:
81   explicit DynamicNumericDiffCostFunction(
82       const CostFunctor* functor,
83       Ownership ownership = TAKE_OWNERSHIP,
84       const NumericDiffOptions& options = NumericDiffOptions())
functor_(functor)85       : functor_(functor), ownership_(ownership), options_(options) {}
86 
~DynamicNumericDiffCostFunction()87   virtual ~DynamicNumericDiffCostFunction() {
88     if (ownership_ != TAKE_OWNERSHIP) {
89       functor_.release();
90     }
91   }
92 
Evaluate(double const * const * parameters,double * residuals,double ** jacobians)93   bool Evaluate(double const* const* parameters,
94                 double* residuals,
95                 double** jacobians) const override {
96     using internal::NumericDiff;
97     CHECK_GT(num_residuals(), 0)
98         << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() "
99         << "before DynamicNumericDiffCostFunction::Evaluate().";
100 
101     const std::vector<int32_t>& block_sizes = parameter_block_sizes();
102     CHECK(!block_sizes.empty())
103         << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() "
104         << "before DynamicNumericDiffCostFunction::Evaluate().";
105 
106     const bool status =
107         internal::VariadicEvaluate<internal::DynamicParameterDims>(
108             *functor_.get(), parameters, residuals);
109     if (jacobians == NULL || !status) {
110       return status;
111     }
112 
113     // Create local space for a copy of the parameters which will get mutated.
114     int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0);
115     std::vector<double> parameters_copy(parameters_size);
116     std::vector<double*> parameters_references_copy(block_sizes.size());
117     parameters_references_copy[0] = &parameters_copy[0];
118     for (size_t block = 1; block < block_sizes.size(); ++block) {
119       parameters_references_copy[block] =
120           parameters_references_copy[block - 1] + block_sizes[block - 1];
121     }
122 
123     // Copy the parameters into the local temp space.
124     for (size_t block = 0; block < block_sizes.size(); ++block) {
125       memcpy(parameters_references_copy[block],
126              parameters[block],
127              block_sizes[block] * sizeof(*parameters[block]));
128     }
129 
130     for (size_t block = 0; block < block_sizes.size(); ++block) {
131       if (jacobians[block] != NULL &&
132           !NumericDiff<CostFunctor,
133                        method,
134                        ceres::DYNAMIC,
135                        internal::DynamicParameterDims,
136                        ceres::DYNAMIC,
137                        ceres::DYNAMIC>::
138               EvaluateJacobianForParameterBlock(functor_.get(),
139                                                 residuals,
140                                                 options_,
141                                                 this->num_residuals(),
142                                                 block,
143                                                 block_sizes[block],
144                                                 &parameters_references_copy[0],
145                                                 jacobians[block])) {
146         return false;
147       }
148     }
149     return true;
150   }
151 
152  private:
153   std::unique_ptr<const CostFunctor> functor_;
154   Ownership ownership_;
155   NumericDiffOptions options_;
156 };
157 
158 }  // namespace ceres
159 
160 #endif  // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
161