c852d57a3fcc123d2b26d532616c1f76b9ef37fd
[blender.git] / extern / ceres / include / ceres / dynamic_numeric_diff_cost_function.h
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 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 // This numeric diff implementation differs from the one found in
35 // numeric_diff_cost_function.h by supporting numericdiff on cost
36 // functions with variable numbers of parameters with variable
37 // sizes. With the other implementation, all the sizes (both the
38 // number of parameter blocks and the size of each block) must be
39 // fixed at compile time.
40 //
41 // The functor API differs slightly from the API for fixed size
42 // numeric diff; the expected interface for the cost functors is:
43 //
44 //   struct MyCostFunctor {
45 //     bool operator()(double const* const* parameters, double* residuals) const {
46 //       // Use parameters[i] to access the i'th parameter block.
47 //     }
48 //   }
49 //
50 // Since the sizing of the parameters is done at runtime, you must
51 // also specify the sizes after creating the
52 // DynamicNumericDiffCostFunction. For example:
53 //
54 //   DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function(
55 //       new MyCostFunctor());
56 //   cost_function.AddParameterBlock(5);
57 //   cost_function.AddParameterBlock(10);
58 //   cost_function.SetNumResiduals(21);
59
60 #ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
61 #define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
62
63 #include <cmath>
64 #include <numeric>
65 #include <vector>
66
67 #include "ceres/cost_function.h"
68 #include "ceres/internal/scoped_ptr.h"
69 #include "ceres/internal/eigen.h"
70 #include "ceres/internal/numeric_diff.h"
71 #include "ceres/numeric_diff_options.h"
72 #include "glog/logging.h"
73
74 namespace ceres {
75
76 template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
77 class DynamicNumericDiffCostFunction : public CostFunction {
78  public:
79   explicit DynamicNumericDiffCostFunction(
80       const CostFunctor* functor,
81       Ownership ownership = TAKE_OWNERSHIP,
82       const NumericDiffOptions& options = NumericDiffOptions())
83       : functor_(functor),
84         ownership_(ownership),
85         options_(options) {
86   }
87
88   // Deprecated. New users should avoid using this constructor. Instead, use the
89   // constructor with NumericDiffOptions.
90   DynamicNumericDiffCostFunction(
91       const CostFunctor* functor,
92       Ownership ownership,
93       double relative_step_size)
94       : functor_(functor),
95         ownership_(ownership),
96         options_() {
97     LOG(WARNING) << "This constructor is deprecated and will be removed in "
98                     "a future version. Please use the NumericDiffOptions "
99                     "constructor instead.";
100
101     options_.relative_step_size = relative_step_size;
102   }
103
104   virtual ~DynamicNumericDiffCostFunction() {
105     if (ownership_ != TAKE_OWNERSHIP) {
106       functor_.release();
107     }
108   }
109
110   void AddParameterBlock(int size) {
111     mutable_parameter_block_sizes()->push_back(size);
112   }
113
114   void SetNumResiduals(int num_residuals) {
115     set_num_residuals(num_residuals);
116   }
117
118   virtual bool Evaluate(double const* const* parameters,
119                         double* residuals,
120                         double** jacobians) const {
121     using internal::NumericDiff;
122     CHECK_GT(num_residuals(), 0)
123         << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() "
124         << "before DynamicNumericDiffCostFunction::Evaluate().";
125
126     const std::vector<int32>& block_sizes = parameter_block_sizes();
127     CHECK(!block_sizes.empty())
128         << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() "
129         << "before DynamicNumericDiffCostFunction::Evaluate().";
130
131     const bool status = EvaluateCostFunctor(parameters, residuals);
132     if (jacobians == NULL || !status) {
133       return status;
134     }
135
136     // Create local space for a copy of the parameters which will get mutated.
137     int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0);
138     std::vector<double> parameters_copy(parameters_size);
139     std::vector<double*> parameters_references_copy(block_sizes.size());
140     parameters_references_copy[0] = &parameters_copy[0];
141     for (int block = 1; block < block_sizes.size(); ++block) {
142       parameters_references_copy[block] = parameters_references_copy[block - 1]
143           + block_sizes[block - 1];
144     }
145
146     // Copy the parameters into the local temp space.
147     for (int block = 0; block < block_sizes.size(); ++block) {
148       memcpy(parameters_references_copy[block],
149              parameters[block],
150              block_sizes[block] * sizeof(*parameters[block]));
151     }
152
153     for (int block = 0; block < block_sizes.size(); ++block) {
154       if (jacobians[block] != NULL &&
155           !NumericDiff<CostFunctor, method, DYNAMIC,
156                        DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
157                        DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
158                        DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock(
159                                              functor_.get(),
160                                              residuals,
161                                              options_,
162                                              this->num_residuals(),
163                                              block,
164                                              block_sizes[block],
165                                              &parameters_references_copy[0],
166                                              jacobians[block])) {
167         return false;
168       }
169     }
170     return true;
171   }
172
173  private:
174   bool EvaluateCostFunctor(double const* const* parameters,
175                            double* residuals) const {
176     return EvaluateCostFunctorImpl(functor_.get(),
177                                    parameters,
178                                    residuals,
179                                    functor_.get());
180   }
181
182   // Helper templates to allow evaluation of a functor or a
183   // CostFunction.
184   bool EvaluateCostFunctorImpl(const CostFunctor* functor,
185                                double const* const* parameters,
186                                double* residuals,
187                                const void* /* NOT USED */) const {
188     return (*functor)(parameters, residuals);
189   }
190
191   bool EvaluateCostFunctorImpl(const CostFunctor* functor,
192                                double const* const* parameters,
193                                double* residuals,
194                                const CostFunction* /* NOT USED */) const {
195     return functor->Evaluate(parameters, residuals, NULL);
196   }
197
198   internal::scoped_ptr<const CostFunctor> functor_;
199   Ownership ownership_;
200   NumericDiffOptions options_;
201 };
202
203 }  // namespace ceres
204
205 #endif  // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_