eigen_quaternion_parameterization.h
Go to the documentation of this file.
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2016 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
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 #ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
32 #define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
33 
34 #include "ceres/local_parameterization.h"
35 
36 namespace ceres {
37 
38 // Implements the quaternion local parameterization for Eigen's representation
39 // of the quaternion. Eigen uses a different internal memory layout for the
40 // elements of the quaternion than what is commonly used. Specifically, Eigen
41 // stores the elements in memory as [x, y, z, w] where the real part is last
42 // whereas it is typically stored first. Note, when creating an Eigen quaternion
43 // through the constructor the elements are accepted in w, x, y, z order. Since
44 // Ceres operates on parameter blocks which are raw double pointers this
45 // difference is important and requires a different parameterization.
46 //
47 // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
48 // with * being the quaternion multiplication operator.
49 class EigenQuaternionParameterization : public ceres::LocalParameterization {
50  public:
52  virtual bool Plus(const double* x_ptr,
53  const double* delta,
54  double* x_plus_delta_ptr) const
55  {
56  Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr);
58 
59  const double norm_delta =
60  sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
61  if (norm_delta > 0.0) {
62  const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
63 
64  // Note, in the constructor w is first.
65  Eigen::Quaterniond delta_q(cos(norm_delta),
66  sin_delta_by_delta * delta[0],
67  sin_delta_by_delta * delta[1],
68  sin_delta_by_delta * delta[2]);
69  x_plus_delta = delta_q * x;
70  } else {
71  x_plus_delta = x;
72  }
73 
74  return true;
75  }
76  virtual bool ComputeJacobian(const double* x,
77  double* jacobian) const{
78  jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT
79  jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT
80  jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT
81  jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT
82  return true;
83  }
84  virtual int GlobalSize() const { return 4; }
85  virtual int LocalSize() const { return 3; }
86 };
87 
88 } // namespace ceres
89 
90 #endif // CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
ceres::EigenQuaternionParameterization::~EigenQuaternionParameterization
virtual ~EigenQuaternionParameterization()
Definition: eigen_quaternion_parameterization.h:51
delta
def delta(g0, g1)
ceres::EigenQuaternionParameterization::ComputeJacobian
virtual bool ComputeJacobian(const double *x, double *jacobian) const
Definition: eigen_quaternion_parameterization.h:76
x
x
ceres
ceres::EigenQuaternionParameterization::LocalSize
virtual int LocalSize() const
Definition: eigen_quaternion_parameterization.h:85
Eigen::Map
ceres::EigenQuaternionParameterization
Definition: eigen_quaternion_parameterization.h:49
Eigen::Quaternion
ceres::EigenQuaternionParameterization::GlobalSize
virtual int GlobalSize() const
Definition: eigen_quaternion_parameterization.h:84
ceres::EigenQuaternionParameterization::Plus
virtual bool Plus(const double *x_ptr, const double *delta, double *x_plus_delta_ptr) const
Definition: eigen_quaternion_parameterization.h:52
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:09