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 // 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) functor_(new NumericDiffCostFunction<Functor,kMethod,kNumResiduals,N0,N1,N2,N3,N4,N5,N6,N7,N8,N9> (new Functor,TAKE_OWNERSHIP,kNumResiduals,relative_step_size))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 TAKE_OWNERSHIP, 128 kNumResiduals, 129 relative_step_size)) { 130 } 131 132 NumericDiffFunctor(Functor* functor, double relative_step_size = 1e-6) functor_(new NumericDiffCostFunction<Functor,kMethod,kNumResiduals,N0,N1,N2,N3,N4,N5,N6,N7,N8,N9> (functor,TAKE_OWNERSHIP,kNumResiduals,relative_step_size))133 : functor_(new NumericDiffCostFunction<Functor, 134 kMethod, 135 kNumResiduals, 136 N0, N1, N2, N3, N4, 137 N5, N6, N7, N8, N9>( 138 functor, 139 TAKE_OWNERSHIP, 140 kNumResiduals, 141 relative_step_size)) { 142 } 143 operator()144 bool operator()(const double* x0, double* residuals) const { 145 return functor_(x0, residuals); 146 } 147 operator()148 bool operator()(const double* x0, 149 const double* x1, 150 double* residuals) const { 151 return functor_(x0, x1, residuals); 152 } 153 operator()154 bool operator()(const double* x0, 155 const double* x1, 156 const double* x2, 157 double* residuals) const { 158 return functor_(x0, x1, x2, residuals); 159 } 160 operator()161 bool operator()(const double* x0, 162 const double* x1, 163 const double* x2, 164 const double* x3, 165 double* residuals) const { 166 return functor_(x0, x1, x2, x3, residuals); 167 } 168 operator()169 bool operator()(const double* x0, 170 const double* x1, 171 const double* x2, 172 const double* x3, 173 const double* x4, 174 double* residuals) const { 175 return functor_(x0, x1, x2, x3, x4, residuals); 176 } 177 operator()178 bool operator()(const double* x0, 179 const double* x1, 180 const double* x2, 181 const double* x3, 182 const double* x4, 183 const double* x5, 184 double* residuals) const { 185 return functor_(x0, x1, x2, x3, x4, x5, residuals); 186 } 187 operator()188 bool operator()(const double* x0, 189 const double* x1, 190 const double* x2, 191 const double* x3, 192 const double* x4, 193 const double* x5, 194 const double* x6, 195 double* residuals) const { 196 return functor_(x0, x1, x2, x3, x4, x5, x6, residuals); 197 } 198 operator()199 bool operator()(const double* x0, 200 const double* x1, 201 const double* x2, 202 const double* x3, 203 const double* x4, 204 const double* x5, 205 const double* x6, 206 const double* x7, 207 double* residuals) const { 208 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals); 209 } 210 operator()211 bool operator()(const double* x0, 212 const double* x1, 213 const double* x2, 214 const double* x3, 215 const double* x4, 216 const double* x5, 217 const double* x6, 218 const double* x7, 219 const double* x8, 220 double* residuals) const { 221 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals); 222 } 223 operator()224 bool operator()(const double* x0, 225 const double* x1, 226 const double* x2, 227 const double* x3, 228 const double* x4, 229 const double* x5, 230 const double* x6, 231 const double* x7, 232 const double* x8, 233 const double* x9, 234 double* residuals) const { 235 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals); 236 } 237 238 template <typename T> operator()239 bool operator()(const T* x0, T* residuals) const { 240 return functor_(x0, residuals); 241 } 242 243 template <typename T> operator()244 bool operator()(const T* x0, 245 const T* x1, 246 T* residuals) const { 247 return functor_(x0, x1, residuals); 248 } 249 250 template <typename T> operator()251 bool operator()(const T* x0, 252 const T* x1, 253 const T* x2, 254 T* residuals) const { 255 return functor_(x0, x1, x2, residuals); 256 } 257 258 template <typename T> operator()259 bool operator()(const T* x0, 260 const T* x1, 261 const T* x2, 262 const T* x3, 263 T* residuals) const { 264 return functor_(x0, x1, x2, x3, residuals); 265 } 266 267 template <typename T> operator()268 bool operator()(const T* x0, 269 const T* x1, 270 const T* x2, 271 const T* x3, 272 const T* x4, 273 T* residuals) const { 274 return functor_(x0, x1, x2, x3, x4, residuals); 275 } 276 277 template <typename T> operator()278 bool operator()(const T* x0, 279 const T* x1, 280 const T* x2, 281 const T* x3, 282 const T* x4, 283 const T* x5, 284 T* residuals) const { 285 return functor_(x0, x1, x2, x3, x4, x5, residuals); 286 } 287 288 template <typename T> operator()289 bool operator()(const T* x0, 290 const T* x1, 291 const T* x2, 292 const T* x3, 293 const T* x4, 294 const T* x5, 295 const T* x6, 296 T* residuals) const { 297 return functor_(x0, x1, x2, x3, x4, x5, x6, residuals); 298 } 299 300 template <typename T> operator()301 bool operator()(const T* x0, 302 const T* x1, 303 const T* x2, 304 const T* x3, 305 const T* x4, 306 const T* x5, 307 const T* x6, 308 const T* x7, 309 T* residuals) const { 310 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals); 311 } 312 313 template <typename T> operator()314 bool operator()(const T* x0, 315 const T* x1, 316 const T* x2, 317 const T* x3, 318 const T* x4, 319 const T* x5, 320 const T* x6, 321 const T* x7, 322 const T* x8, 323 T* residuals) const { 324 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals); 325 } 326 327 template <typename T> operator()328 bool operator()(const T* x0, 329 const T* x1, 330 const T* x2, 331 const T* x3, 332 const T* x4, 333 const T* x5, 334 const T* x6, 335 const T* x7, 336 const T* x8, 337 const T* x9, 338 T* residuals) const { 339 return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals); 340 } 341 342 343 private: 344 CostFunctionToFunctor<kNumResiduals, 345 N0, N1, N2, N3, N4, 346 N5, N6, N7, N8, N9> functor_; 347 }; 348 349 } // namespace ceres 350 351 #endif // CERES_PUBLIC_NUMERIC_DIFF_FUNCTOR_H_ 352