pose_graph_3d_error_term.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: vitus@google.com (Michael Vitus)
30 
31 #ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
32 #define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
33 
34 #include "Eigen/Core"
35 #include "ceres/autodiff_cost_function.h"
36 
37 #include "types.h"
38 
39 namespace ceres {
40 namespace examples {
41 
42 // Computes the error term for two poses that have a relative pose measurement
43 // between them. Let the hat variables be the measurement. We have two poses x_a
44 // and x_b. Through sensor measurements we can measure the transformation of
45 // frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
46 // between the current estimate of the poses and the measurement.
47 //
48 // In this formulation, we have chosen to represent the rigid transformation as
49 // a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
50 // [x, y, z, w].
51 
52 // The estimated measurement is:
53 // t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
54 // [ q_ab ] [ q_a^{-1] * q_b ]
55 //
56 // where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
57 // quaternion. Now we can compute an error metric between the estimated and
58 // measurement transformation. For the orientation error, we will use the
59 // standard multiplicative error resulting in:
60 //
61 // error = [ p_ab - \hat{p}_ab ]
62 // [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
63 //
64 // where Vec(*) returns the vector (imaginary) part of the quaternion. Since
65 // the measurement has an uncertainty associated with how accurate it is, we
66 // will weight the errors by the square root of the measurement information
67 // matrix:
68 //
69 // residuals = I^{1/2) * error
70 // where I is the information matrix which is the inverse of the covariance.
72  public:
73  PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
74  const Eigen::Matrix<double, 6, 6>& sqrt_information)
75  : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
76 
77  template <typename T>
78  bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
79  const T* const p_b_ptr, const T* const q_b_ptr,
80  T* residuals_ptr) const {
83 
86 
87  // Compute the relative transformation between the two frames.
88  Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
89  Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
90 
91  // Represent the displacement between the two frames in the A frame.
92  Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
93 
94  // Compute the error between the two orientation estimates.
95  Eigen::Quaternion<T> delta_q =
96  t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
97 
98  // Compute the residuals.
99  // [ position ] [ delta_p ]
100  // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
101  Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
102  residuals.template block<3, 1>(0, 0) =
103  p_ab_estimated - t_ab_measured_.p.template cast<T>();
104  residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
105 
106  // Scale the residuals by the measurement uncertainty.
107  residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
108 
109  return true;
110  }
111 
112  static ceres::CostFunction* Create(
113  const Pose3d& t_ab_measured,
114  const Eigen::Matrix<double, 6, 6>& sqrt_information) {
115  return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
116  new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
117  }
118 
120 
121  private:
122  // The measurement for the position of B relative to A in the A frame.
124  // The square root of the measurement information matrix.
126 };
127 
128 } // namespace examples
129 } // namespace ceres
130 
131 #endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
ceres::examples::Pose3d
Definition: pose_graph_3d/types.h:45
ceres::examples::PoseGraph3dErrorTerm
Definition: pose_graph_3d_error_term.h:71
ceres::examples::PoseGraph3dErrorTerm::PoseGraph3dErrorTerm
PoseGraph3dErrorTerm(const Pose3d &t_ab_measured, const Eigen::Matrix< double, 6, 6 > &sqrt_information)
Definition: pose_graph_3d_error_term.h:73
types.h
Eigen::Triplet< double >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ceres
Eigen::Map
Eigen::Quaternion
ceres::examples::Pose3d::q
Eigen::Quaterniond q
Definition: pose_graph_3d/types.h:47
ceres::examples::PoseGraph3dErrorTerm::t_ab_measured_
const Pose3d t_ab_measured_
Definition: pose_graph_3d_error_term.h:123
Eigen::Matrix< double, 6, 6 >
ceres::examples::Pose3d::p
Eigen::Vector3d p
Definition: pose_graph_3d/types.h:46
ceres::examples::PoseGraph3dErrorTerm::operator()
bool operator()(const T *const p_a_ptr, const T *const q_a_ptr, const T *const p_b_ptr, const T *const q_b_ptr, T *residuals_ptr) const
Definition: pose_graph_3d_error_term.h:78
ceres::examples::PoseGraph3dErrorTerm::sqrt_information_
const Eigen::Matrix< double, 6, 6 > sqrt_information_
Definition: pose_graph_3d_error_term.h:125
ceres::examples::PoseGraph3dErrorTerm::Create
static ceres::CostFunction * Create(const Pose3d &t_ab_measured, const Eigen::Matrix< double, 6, 6 > &sqrt_information)
Definition: pose_graph_3d_error_term.h:112


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