cartogapher 源码分析 (transform)

分析子文件夹transform的内容

proto

首先打开.proto去查看依赖的数据结构，可以发现google就是爱造轮子，连基本的vector和四元数也自定义。

主要分为三种数据

变量 用途 Vector 表示移动 Quaternion 表示旋转 Rigid 封装了移动和旋转

所有坐标系都是右手坐标系，x前，y左，z上。和ros一样。

eigen库和这个刚好相反（这个不太了解，需要去测试一些eigen的函数，学习学习）

syntax = "proto2";package cartographer.transform.proto;// All coordinates are expressed in the right-handed Cartesian coordinate system// used by Cartographer (x forward, y left, z up). Message names are chosen to// mirror those used in the Eigen library.message Vector2d {  optional double x = 1;  optional double y = 2;}message Vector2f {  optional float x = 1;  optional float y = 2;}message Vector3d {  optional double x = 1;  optional double y = 2;  optional double z = 3;}message Vector3f {  optional float x = 1;  optional float y = 2;  optional float z = 3;}message Quaterniond {  optional double x = 1;  optional double y = 2;  optional double z = 3;  optional double w = 4;}message Quaternionf {  optional float x = 1;  optional float y = 2;  optional float z = 3;  optional float w = 4;}message Rigid2d {  optional Vector2d translation = 1;  optional double rotation = 2;}message Rigid2f {  optional Vector2f translation = 1;  optional float rotation = 2;}message Rigid3d {  optional Vector3d translation = 1;  optional Quaterniond rotation = 2;}message Rigid3f {  optional Vector3f translation = 1;  optional Quaternionf rotation = 2;}

transform

（transform.cc和transform.h是由transform.proto
自动生成的，不用多看。）
上面是一开始的理解，后来发现是错的，因为由proto生成的文件，在名字上还会多一个pb。还是要更仔细。
里面主要是四元数和欧拉角的转化，以及eigen下的向量 四元数和proto里的数据转化。

rigid_transform

其中定义了基于rigid数据类型的一些转化，乘法操作，逆矩阵等。分别实现了2d和3d坐标系下RT矩阵的操作。
下面贴出了其对应的googletest代码。

EXPECT_NEAR是用来比较两个浮点数是否相等。

#include "cartographer/transform/transform.h"#include <random>#include "cartographer/transform/rigid_transform.h"#include "cartographer/transform/rigid_transform_test_helpers.h"#include "gtest/gtest.h"namespace cartographer {namespace transform {namespace {TEST(TransformTest, GetAngle) {  std::mt19937 rng(42);  std::uniform_real_distribution<float> angle_distribution(0.f, M_PI);  std::uniform_real_distribution<float> position_distribution(-1.f, 1.f);  for (int i = 0; i != 100; ++i) {    const float angle = angle_distribution(rng);    const float x = position_distribution(rng);    const float y = position_distribution(rng);    const float z = position_distribution(rng);    const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized();    EXPECT_NEAR(angle,                GetAngle(Rigid3f::Rotation(AngleAxisVectorToRotationQuaternion(                    Eigen::Vector3f(angle * axis)))),                1e-6f);  }}TEST(TransformTest, GetYaw) {  const auto rotation =      Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()));  EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);  EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9);}TEST(TransformTest, GetYawAxisOrdering) {  const auto rotation =      Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) *                        Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) *                        Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX()));  EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);}TEST(TransformTest, Embed3D) {  const Rigid2d rigid2d({1., 2.}, 0.);  const Rigid3d rigid3d(      Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *      Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized()));  const Rigid3d expected =      rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.));  EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9));}}  // namespace}  // namespace transform}  // namespace cartographer

transform_interpolation_buffer.h

只定义了一个class，成员变量是

 private:  struct TimestampedTransform {    common::Time time;    transform::Rigid3d transform;  };  std::deque<TimestampedTransform> deque_;

暂时不清楚为何只有Rigid3d这个数据类型，应该也有2d的类型。
该数据结构定义了一个时间和坐标系转化一一对应的缓冲池，因为是按时间序列排列，所以使用了双向队列deque的数据类型。这种类型的确很适合用于基于时间的滑动窗口！
下面贴出了其对应的googletest代码。

#include "cartographer/transform/transform.h"#include <random>#include "cartographer/transform/rigid_transform.h"#include "cartographer/transform/rigid_transform_test_helpers.h"#include "gtest/gtest.h"namespace cartographer {namespace transform {namespace {TEST(TransformTest, GetAngle) {  std::mt19937 rng(42);  std::uniform_real_distribution<float> angle_distribution(0.f, M_PI);  std::uniform_real_distribution<float> position_distribution(-1.f, 1.f);  for (int i = 0; i != 100; ++i) {    const float angle = angle_distribution(rng);    const float x = position_distribution(rng);    const float y = position_distribution(rng);    const float z = position_distribution(rng);    const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized();    EXPECT_NEAR(angle,                GetAngle(Rigid3f::Rotation(AngleAxisVectorToRotationQuaternion(                    Eigen::Vector3f(angle * axis)))),                1e-6f);  }}TEST(TransformTest, GetYaw) {  const auto rotation =      Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()));  EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);  EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9);}TEST(TransformTest, GetYawAxisOrdering) {  const auto rotation =      Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) *                        Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) *                        Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX()));  EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9);}TEST(TransformTest, Embed3D) {  const Rigid2d rigid2d({1., 2.}, 0.);  const Rigid3d rigid3d(      Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *      Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized()));  const Rigid3d expected =      rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.));  EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9));}}  // namespace}  // namespace transform}  // namespace cartographer

之所以只贴gtest代码，是因为我们要先理解函数怎么用，再去剖析函数的具体实现。对于这个大项目，能先把每个函数怎么调用搞清楚，已经要花不少时间了。。。