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 109namespace ceres { 110 111template<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> 116class 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 TAKE_OWNERSHIP, 128 kNumResiduals, 129 relative_step_size)) { 130 } 131 132 NumericDiffFunctor(Functor* functor, double relative_step_size = 1e-6) 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 144 bool operator()(const double* x0, double* residuals) const { 145 return functor_(x0, residuals); 146 } 147 148 bool operator()(const double* x0, 149 const double* x1, 150 double* residuals) const { 151 return functor_(x0, x1, residuals); 152 } 153 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 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 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 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 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 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 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 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> 239 bool operator()(const T* x0, T* residuals) const { 240 return functor_(x0, residuals); 241 } 242 243 template <typename T> 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> 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> 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> 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> 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> 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> 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> 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> 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