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: sergey.vfx@gmail.com (Sergey Sharybin)
30 //         mierle@gmail.com (Keir Mierle)
31 //         sameeragarwal@google.com (Sameer Agarwal)
32 
33 #ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
34 #define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
35 
36 #include "ceres/internal/autodiff.h"
37 #include "ceres/internal/scoped_ptr.h"
38 #include "ceres/local_parameterization.h"
39 
40 namespace ceres {
41 
42 // Create local parameterization with Jacobians computed via automatic
43 // differentiation. For more information on local parameterizations,
44 // see include/ceres/local_parameterization.h
45 //
46 // To get an auto differentiated local parameterization, you must define
47 // a class with a templated operator() (a functor) that computes
48 //
49 //   x_plus_delta = Plus(x, delta);
50 //
51 // the template parameter T. The autodiff framework substitutes appropriate
52 // "Jet" objects for T in order to compute the derivative when necessary, but
53 // this is hidden, and you should write the function as if T were a scalar type
54 // (e.g. a double-precision floating point number).
55 //
56 // The function must write the computed value in the last argument (the only
57 // non-const one) and return true to indicate success.
58 //
59 // For example, Quaternions have a three dimensional local
60 // parameterization. It's plus operation can be implemented as (taken
61 // from internal/ceres/auto_diff_local_parameterization_test.cc)
62 //
63 //   struct QuaternionPlus {
64 //     template<typename T>
65 //     bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
66 //       const T squared_norm_delta =
67 //           delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
68 //
69 //       T q_delta[4];
70 //       if (squared_norm_delta > T(0.0)) {
71 //         T norm_delta = sqrt(squared_norm_delta);
72 //         const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
73 //         q_delta[0] = cos(norm_delta);
74 //         q_delta[1] = sin_delta_by_delta * delta[0];
75 //         q_delta[2] = sin_delta_by_delta * delta[1];
76 //         q_delta[3] = sin_delta_by_delta * delta[2];
77 //       } else {
78 //         // We do not just use q_delta = [1,0,0,0] here because that is a
79 //         // constant and when used for automatic differentiation will
80 //         // lead to a zero derivative. Instead we take a first order
81 //         // approximation and evaluate it at zero.
82 //         q_delta[0] = T(1.0);
83 //         q_delta[1] = delta[0];
84 //         q_delta[2] = delta[1];
85 //         q_delta[3] = delta[2];
86 //       }
87 //
88 //       QuaternionProduct(q_delta, x, x_plus_delta);
89 //       return true;
90 //     }
91 //   };
92 //
93 // Then given this struct, the auto differentiated local
94 // parameterization can now be constructed as
95 //
96 //   LocalParameterization* local_parameterization =
97 //     new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>;
98 //                                                       |  |
99 //                            Global Size ---------------+  |
100 //                            Local Size -------------------+
101 //
102 // WARNING: Since the functor will get instantiated with different types for
103 // T, you must to convert from other numeric types to T before mixing
104 // computations with other variables of type T. In the example above, this is
105 // seen where instead of using k_ directly, k_ is wrapped with T(k_).
106 
107 template <typename Functor, int kGlobalSize, int kLocalSize>
108 class AutoDiffLocalParameterization : public LocalParameterization {
109  public:
AutoDiffLocalParameterization()110   AutoDiffLocalParameterization() :
111       functor_(new Functor()) {}
112 
113   // Takes ownership of functor.
AutoDiffLocalParameterization(Functor * functor)114   explicit AutoDiffLocalParameterization(Functor* functor) :
115       functor_(functor) {}
116 
~AutoDiffLocalParameterization()117   virtual ~AutoDiffLocalParameterization() {}
Plus(const double * x,const double * delta,double * x_plus_delta)118   virtual bool Plus(const double* x,
119                     const double* delta,
120                     double* x_plus_delta) const {
121     return (*functor_)(x, delta, x_plus_delta);
122   }
123 
ComputeJacobian(const double * x,double * jacobian)124   virtual bool ComputeJacobian(const double* x, double* jacobian) const {
125     double zero_delta[kLocalSize];
126     for (int i = 0; i < kLocalSize; ++i) {
127       zero_delta[i] = 0.0;
128     }
129 
130     double x_plus_delta[kGlobalSize];
131     for (int i = 0; i < kGlobalSize; ++i) {
132       x_plus_delta[i] = 0.0;
133     }
134 
135     const double* parameter_ptrs[2] = {x, zero_delta};
136     double* jacobian_ptrs[2] = { NULL, jacobian };
137     return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize>
138         ::Differentiate(*functor_,
139                         parameter_ptrs,
140                         kGlobalSize,
141                         x_plus_delta,
142                         jacobian_ptrs);
143   }
144 
GlobalSize()145   virtual int GlobalSize() const { return kGlobalSize; }
LocalSize()146   virtual int LocalSize() const { return kLocalSize; }
147 
148  private:
149   internal::scoped_ptr<Functor> functor_;
150 };
151 
152 }  // namespace ceres
153 
154 #endif  // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
155