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
40namespace 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
107template <typename Functor, int kGlobalSize, int kLocalSize>
108class AutoDiffLocalParameterization : public LocalParameterization {
109 public:
110  AutoDiffLocalParameterization() :
111      functor_(new Functor()) {}
112
113  // Takes ownership of functor.
114  explicit AutoDiffLocalParameterization(Functor* functor) :
115      functor_(functor) {}
116
117  virtual ~AutoDiffLocalParameterization() {}
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
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
145  virtual int GlobalSize() const { return kGlobalSize; }
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