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代碼，是因爲我們要先理解函數怎麼用，再去剖析函數的具體實現。對於這個大項目，能先把每個函數怎麼調用搞清楚，已經要花不少時間了。。。