ceres_utils.h
Go to the documentation of this file.
1 /*
2  * Copyright 2018 Simbe Robotics
3  * Author: Steve Macenski
4  */
5 
6 #include <ceres/ceres.h>
7 #include <ceres/local_parameterization.h>
8 #include <cmath>
9 #include <utility>
10 
11 /*****************************************************************************/
12 /*****************************************************************************/
13 /*****************************************************************************/
14 inline std::size_t GetHash(const int& x, const int& y)
15 {
16  return ((std::hash<double>()(x) ^ (std::hash<double>()(y) << 1)) >> 1);
17 }
18 
19 /*****************************************************************************/
20 /*****************************************************************************/
21 /*****************************************************************************/
22 
23 // Normalizes the angle in radians between [-pi and pi).
24 template <typename T> inline T NormalizeAngle(const T& angle_radians)
25 {
26  T two_pi(2.0 * M_PI);
27  return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
28 }
29 
30 /*****************************************************************************/
31 /*****************************************************************************/
32 /*****************************************************************************/
33 
35 {
36  public:
37  template <typename T>
38  bool operator()(const T* theta_radians, const T* delta_theta_radians, T* theta_radians_plus_delta) const
39  {
40  *theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians);
41  return true;
42  }
43 
44  static ceres::LocalParameterization* Create()
45  {
46  return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
47  }
48 };
49 
50 /*****************************************************************************/
51 /*****************************************************************************/
52 /*****************************************************************************/
53 
54 template <typename T>
55 Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians)
56 {
57  const T cos_yaw = ceres::cos(yaw_radians);
58  const T sin_yaw = ceres::sin(yaw_radians);
59  Eigen::Matrix<T, 2, 2> rotation;
60  rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
61  return rotation;
62 }
63 
64 /*****************************************************************************/
65 /*****************************************************************************/
66 /*****************************************************************************/
67 
69 {
70  public:
71  PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information)
72  : p_ab_(x_ab, y_ab), yaw_ab_radians_(yaw_ab_radians), sqrt_information_(sqrt_information)
73  {
74  }
75 
76  template <typename T>
77  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
78  {
79  const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
80  const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
81  Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
82  residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
83  residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
84  // Scale the residuals by the square root information matrix to account for the measurement uncertainty.
85  residuals_map = sqrt_information_.template cast<T>() * residuals_map;
86  return true;
87  }
88 
89  static ceres::CostFunction* Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information)
90  {
91  return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information)));
92  }
93 
94  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
95  private:
96  // The position of B relative to A in the A frame.
97  const Eigen::Vector2d p_ab_;
98  // The orientation of frame B relative to frame A.
99  const double yaw_ab_radians_;
100  // The inverse square root of the measurement covariance matrix.
101  const Eigen::Matrix3d sqrt_information_;
102 };
103 
PoseGraph2dErrorTerm::p_ab_
const Eigen::Vector2d p_ab_
Definition: ceres_utils.h:97
AngleLocalParameterization
Definition: ceres_utils.h:34
GetHash
std::size_t GetHash(const int &x, const int &y)
Definition: ceres_utils.h:14
NormalizeAngle
T NormalizeAngle(const T &angle_radians)
Definition: ceres_utils.h:24
RotationMatrix2D
Eigen::Matrix< T, 2, 2 > RotationMatrix2D(T yaw_radians)
Definition: ceres_utils.h:55
AngleLocalParameterization::operator()
bool operator()(const T *theta_radians, const T *delta_theta_radians, T *theta_radians_plus_delta) const
Definition: ceres_utils.h:38
PoseGraph2dErrorTerm
Definition: ceres_utils.h:68
PoseGraph2dErrorTerm::PoseGraph2dErrorTerm
PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d &sqrt_information)
Definition: ceres_utils.h:71
PoseGraph2dErrorTerm::yaw_ab_radians_
const double yaw_ab_radians_
Definition: ceres_utils.h:99
PoseGraph2dErrorTerm::Create
static ceres::CostFunction * Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d &sqrt_information)
Definition: ceres_utils.h:89
AngleLocalParameterization::Create
static ceres::LocalParameterization * Create()
Definition: ceres_utils.h:44
PoseGraph2dErrorTerm::sqrt_information_
const Eigen::Matrix3d sqrt_information_
Definition: ceres_utils.h:101
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: ceres_utils.h:77


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55