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] = ¶meters_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 ¶meters_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