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 // 31 // A wrapper class that takes a variadic functor evaluating a 32 // function, numerically differentiates it and makes it available as a 33 // templated functor so that it can be easily used as part of Ceres' 34 // automatic differentiation framework. 35 // 36 // For example: 37 // 38 // For example, let us assume that 39 // 40 // struct IntrinsicProjection 41 // IntrinsicProjection(const double* observations); 42 // bool operator()(const double* calibration, 43 // const double* point, 44 // double* residuals); 45 // }; 46 // 47 // is a functor that implements the projection of a point in its local 48 // coordinate system onto its image plane and subtracts it from the 49 // observed point projection. 50 // 51 // Now we would like to compose the action of this functor with the 52 // action of camera extrinsics, i.e., rotation and translation, which 53 // is given by the following templated function 54 // 55 // template<typename T> 56 // void RotateAndTranslatePoint(const T* rotation, 57 // const T* translation, 58 // const T* point, 59 // T* result); 60 // 61 // To compose the extrinsics and intrinsics, we can construct a 62 // CameraProjection functor as follows. 63 // 64 // struct CameraProjection { 65 // typedef NumericDiffFunctor<IntrinsicProjection, CENTRAL, 2, 5, 3> 66 // IntrinsicProjectionFunctor; 67 // 68 // CameraProjection(double* observation) { 69 // intrinsic_projection_.reset( 70 // new IntrinsicProjectionFunctor(observation)) { 71 // } 72 // 73 // template <typename T> 74 // bool operator()(const T* rotation, 75 // const T* translation, 76 // const T* intrinsics, 77 // const T* point, 78 // T* residuals) const { 79 // T transformed_point[3]; 80 // RotateAndTranslatePoint(rotation, translation, point, transformed_point); 81 // return (*intrinsic_projection_)(intrinsics, transformed_point, residual); 82 // } 83 // 84 // private: 85 // scoped_ptr<IntrinsicProjectionFunctor> intrinsic_projection_; 86 // }; 87 // 88 // Here, we made the choice of using CENTRAL differences to compute 89 // the jacobian of IntrinsicProjection. 90 // 91 // Now, we are ready to construct an automatically differentiated cost 92 // function as 93 // 94 // CostFunction* cost_function = 95 // new AutoDiffCostFunction<CameraProjection, 2, 3, 3, 5>( 96 // new CameraProjection(observations)); 97 // 98 // cost_function now seamlessly integrates automatic differentiation 99 // of RotateAndTranslatePoint with a numerically differentiated 100 // version of IntrinsicProjection. 101 102 #ifndef CERES_PUBLIC_NUMERIC_DIFF_FUNCTOR_H_ 103 #define CERES_PUBLIC_NUMERIC_DIFF_FUNCTOR_H_ 104 105 #include "ceres/numeric_diff_cost_function.h" 106 #include "ceres/types.h" 107 #include "ceres/cost_function_to_functor.h" 108 109 namespace ceres { 110 111 template<typename Functor, 112 NumericDiffMethod kMethod = CENTRAL, 113 int kNumResiduals = 0, 114 int N0 = 0, int N1 = 0 , int N2 = 0, int N3 = 0, int N4 = 0, 115 int N5 = 0, int N6 = 0 , int N7 = 0, int N8 = 0, int N9 = 0> 116 class NumericDiffFunctor { 117 public: 118 // relative_step_size controls the step size used by the numeric 119 // differentiation process. 120 explicit NumericDiffFunctor(double relative_step_size = 1e-6) 121 : functor_( 122 new NumericDiffCostFunction<Functor, 123 kMethod, 124 kNumResiduals, 125 N0, N1, N2, N3, N4, 126 N5, N6, N7, N8, N9>(new Functor, 127 relative_step_size)) { 128 } 129 130 NumericDiffFunctor(Functor* functor, double relative_step_size = 1e-6) 131 : functor_(new NumericDiffCostFunction<Functor, 132 kMethod, 133 kNumResiduals, 134 N0, N1, N2, N3, N4, 135 N5, N6, N7, N8, N9>( 136 functor, relative_step_size)) { 137 } 138 139 bool operator()(const double* x0, double* residuals) const { 140 return functor_(x0, residuals); 141 } 142 143 bool operator()(const double* x0, 144 const double* x1, 145 double* residuals) const { 146 return functor_(x0, x1, residuals); 147 } 148 149 bool operator()(const double* x0, 150 const double* x1, 151 const double* x2, 152 double* residuals) const { 153 return functor_(x0, x1, x2, residuals); 154 } 155 156 bool operator()(const double* x0, 157 const double* x1, 158 const double* x2, 159 const double* x3, 160 double* residuals) const { 161 return functor_(x0, x1, x2, x3, residuals); 162 } 163 164 bool operator()(const double* x0, 165 const double* x1, 166 const double* x2, 167 const double* x3, 168 const double* x4, 169 double* residuals) const { 170 return functor_(x0, x1, x2, x3, x4, residuals); 171 } 172 173 bool operator()(const double* x0, 174 const double* x1, 175 const double* x2, 176 const double* x3, 177 const double* x4, 178 const double* x5, 179 double* residuals) const { 180 return functor_(x0, x1, x2, x3, x4, x5, residuals); 181 } 182 183 bool operator()(const double* x0, 184 const double* x1, 185 const double* x2, 186 const double* x3, 187 const double* x4, 188 const double* x5, 189 const double* x6, 190 double* residuals) const { 191 return functor_(x0, x1, x2, x3, x4, x5, x6, residuals); 192 } 193 194 bool operator()(const double* x0, 195 const double* x1, 196 const double* x2, 197 const double* x3, 198 const double* x4, 199 const double* x5, 200 const double* x6, 201 const double* x7, 202 double* residuals) const { 203 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals); 204 } 205 206 bool operator()(const double* x0, 207 const double* x1, 208 const double* x2, 209 const double* x3, 210 const double* x4, 211 const double* x5, 212 const double* x6, 213 const double* x7, 214 const double* x8, 215 double* residuals) const { 216 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals); 217 } 218 219 bool operator()(const double* x0, 220 const double* x1, 221 const double* x2, 222 const double* x3, 223 const double* x4, 224 const double* x5, 225 const double* x6, 226 const double* x7, 227 const double* x8, 228 const double* x9, 229 double* residuals) const { 230 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals); 231 } 232 233 template <typename T> 234 bool operator()(const T* x0, T* residuals) const { 235 return functor_(x0, residuals); 236 } 237 238 template <typename T> 239 bool operator()(const T* x0, 240 const T* x1, 241 T* residuals) const { 242 return functor_(x0, x1, residuals); 243 } 244 245 template <typename T> 246 bool operator()(const T* x0, 247 const T* x1, 248 const T* x2, 249 T* residuals) const { 250 return functor_(x0, x1, x2, residuals); 251 } 252 253 template <typename T> 254 bool operator()(const T* x0, 255 const T* x1, 256 const T* x2, 257 const T* x3, 258 T* residuals) const { 259 return functor_(x0, x1, x2, x3, residuals); 260 } 261 262 template <typename T> 263 bool operator()(const T* x0, 264 const T* x1, 265 const T* x2, 266 const T* x3, 267 const T* x4, 268 T* residuals) const { 269 return functor_(x0, x1, x2, x3, x4, residuals); 270 } 271 272 template <typename T> 273 bool operator()(const T* x0, 274 const T* x1, 275 const T* x2, 276 const T* x3, 277 const T* x4, 278 const T* x5, 279 T* residuals) const { 280 return functor_(x0, x1, x2, x3, x4, x5, residuals); 281 } 282 283 template <typename T> 284 bool operator()(const T* x0, 285 const T* x1, 286 const T* x2, 287 const T* x3, 288 const T* x4, 289 const T* x5, 290 const T* x6, 291 T* residuals) const { 292 return functor_(x0, x1, x2, x3, x4, x5, x6, residuals); 293 } 294 295 template <typename T> 296 bool operator()(const T* x0, 297 const T* x1, 298 const T* x2, 299 const T* x3, 300 const T* x4, 301 const T* x5, 302 const T* x6, 303 const T* x7, 304 T* residuals) const { 305 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals); 306 } 307 308 template <typename T> 309 bool operator()(const T* x0, 310 const T* x1, 311 const T* x2, 312 const T* x3, 313 const T* x4, 314 const T* x5, 315 const T* x6, 316 const T* x7, 317 const T* x8, 318 T* residuals) const { 319 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals); 320 } 321 322 template <typename T> 323 bool operator()(const T* x0, 324 const T* x1, 325 const T* x2, 326 const T* x3, 327 const T* x4, 328 const T* x5, 329 const T* x6, 330 const T* x7, 331 const T* x8, 332 const T* x9, 333 T* residuals) const { 334 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals); 335 } 336 337 338 private: 339 CostFunctionToFunctor<kNumResiduals, 340 N0, N1, N2, N3, N4, 341 N5, N6, N7, N8, N9> functor_; 342 }; 343 344 } // namespace ceres 345 346 #endif // CERES_PUBLIC_NUMERIC_DIFF_FUNCTOR_H_ 347
