cartographer源码分析(14)-transform-transform.h

最新推荐文章于 2023-04-12 20:41:08 发布

原创

最新推荐文章于 2023-04-12 20:41:08 发布 · 1.6k 阅读

3 ·

CC 4.0 BY-SA版权

本文详细分析了Cartographer项目中的transform-transform.h源码，探讨了其在SLAM（Simultaneous Localization And Mapping）中的作用。源码可以在提供的GitHub链接下载。文章作者分享了对关键功能的理解，并在多个平台发布。

源码可在https://github.com/learnmoreonce/SLAM 下载


文件:transform/transform.h

#ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_H_
#define CARTOGRAPHER_TRANSFORM_TRANSFORM_H_

#include <cmath>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/transform/proto/transform.pb.h"
#include "cartographer/transform/rigid_transform.h"

namespace cartographer {
namespace transform {

//google采用三维 xyz:roll, pitch, yaw

/*
 atan2:返回一个非负的弧度值,  double atan2(double y,double x) :计算复数 x+yi 的辐角
 vec()：a vector expression of the imaginary part (x,y,z) 
 norm()： for vectors, the l2 norm of *this, for matrices the Frobenius norm。
 w()  : the w coefficient

 为何乘以2: 四元数q=[cos(θ/2),sin(θ/2)x,sin(θ/2)y,sin(θ/2)z]
*/


// Returns the non-negative rotation angle in radians of the 3D transformation
// 'transform'.
template <typename FloatType>
FloatType GetAngle(const Rigid3<FloatType>& transform) {
  return FloatType(2) * std::atan2(transform.rotation().vec().norm(),
                                   std::abs(transform.rotation().w()));
}
/*
返回3D变换yaw方向的弧度值,也就是z轴方向的弧度。

*/
// Returns the yaw component in radians of the given 3D 'rotation'. Assuming
// 'rotation' is composed of three rotations around X, then Y, then Z, returns
// the angle of the Z rotation.
template <typename T>
T GetYaw(const Eigen::Quaternion<T>& rotation) {
  const Eigen::Matrix<T, 3, 1> direction =
      rotation * Eigen::Matrix<T, 3, 1>::UnitX();
  return atan2(direction.y(), direction.x());
}

/*
返回3D变换yaw方向(z轴)的弧度值
*/
// Returns the yaw component in radians of the given 3D transformation
// 'transform'.
template <typename T>
T GetYaw(const Rigid3<T>& transform) {
  return GetYaw(transform.rotat