pose_graph_2d_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 // Cost function for a 2D pose graph formulation.
32 
33 #ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
34 #define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
35 
36 #include "Eigen/Core"
37 
38 namespace ceres {
39 namespace examples {
40 
41 template <typename T>
43  const T cos_yaw = ceres::cos(yaw_radians);
44  const T sin_yaw = ceres::sin(yaw_radians);
45 
47  rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
48  return rotation;
49 }
50 
51 // Computes the error term for two poses that have a relative pose measurement
52 // between them. Let the hat variables be the measurement.
53 //
54 // residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
55 // [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
56 //
57 // where r_a is the rotation matrix that rotates a vector represented in frame A
58 // into the global frame, and Normalize(*) ensures the angles are in the range
59 // [-pi, pi).
61  public:
62  PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians,
63  const Eigen::Matrix3d& sqrt_information)
64  : p_ab_(x_ab, y_ab),
65  yaw_ab_radians_(yaw_ab_radians),
66  sqrt_information_(sqrt_information) {}
67 
68  template <typename T>
69  bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a,
70  const T* const x_b, const T* const y_b, const T* const yaw_b,
71  T* residuals_ptr) const {
72  const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
73  const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
74 
75  Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
76 
77  residuals_map.template head<2>() =
78  RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) -
79  p_ab_.cast<T>();
80  residuals_map(2) = ceres::examples::NormalizeAngle(
81  (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
82 
83  // Scale the residuals by the square root information matrix to account for
84  // the measurement uncertainty.
85  residuals_map = sqrt_information_.template cast<T>() * residuals_map;
86 
87  return true;
88  }
89 
90  static ceres::CostFunction* Create(double x_ab, double y_ab,
91  double yaw_ab_radians,
92  const Eigen::Matrix3d& sqrt_information) {
93  return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1,
94  1, 1>(new PoseGraph2dErrorTerm(
95  x_ab, y_ab, yaw_ab_radians, sqrt_information)));
96  }
97 
99 
100  private:
101  // The position of B relative to A in the A frame.
102  const Eigen::Vector2d p_ab_;
103  // The orientation of frame B relative to frame A.
104  const double yaw_ab_radians_;
105  // The inverse square root of the measurement covariance matrix.
106  const Eigen::Matrix3d sqrt_information_;
107 };
108 
109 } // namespace examples
110 } // namespace ceres
111 
112 #endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
ceres::examples::NormalizeAngle
T NormalizeAngle(const T &angle_radians)
Definition: normalize_angle.h:57
ceres::examples::PoseGraph2dErrorTerm::operator()
bool operator()(const T *const x_a, const T *const y_a, const T *const yaw_a, const T *const x_b, const T *const y_b, const T *const yaw_b, T *residuals_ptr) const
Definition: pose_graph_2d_error_term.h:69
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
ceres::examples::PoseGraph2dErrorTerm::yaw_ab_radians_
const double yaw_ab_radians_
Definition: pose_graph_2d_error_term.h:104
ceres::examples::PoseGraph2dErrorTerm::sqrt_information_
const Eigen::Matrix3d sqrt_information_
Definition: pose_graph_2d_error_term.h:106
ceres::examples::PoseGraph2dErrorTerm::Create
static ceres::CostFunction * Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d &sqrt_information)
Definition: pose_graph_2d_error_term.h:90
ceres::examples::PoseGraph2dErrorTerm::PoseGraph2dErrorTerm
PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d &sqrt_information)
Definition: pose_graph_2d_error_term.h:62
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Eigen::Triplet< double >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ceres
Eigen::Map
ceres::examples::PoseGraph2dErrorTerm::p_ab_
const Eigen::Vector2d p_ab_
Definition: pose_graph_2d_error_term.h:102
ceres::examples::PoseGraph2dErrorTerm
Definition: pose_graph_2d_error_term.h:60
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
ceres::examples::RotationMatrix2D
Eigen::Matrix< T, 2, 2 > RotationMatrix2D(T yaw_radians)
Definition: pose_graph_2d_error_term.h:42
Eigen::Matrix


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