[リー群リー代数] Sophus ライブラリの SE3 タイプ テスト (manif と sophus の比較付き)

テストデモ

試験結果

Sophus ライブラリの SE3 クラスに対して、リー群プロパティ、元のデータ アクセス、突然変異アクセサー、コンストラクター、フィッティングなどの一連のテストを実行します。各テストでは、あらかじめ設定されたデータを使用して動作し、SOPHUS_TEST_APPROX や SOPHUS_TEST_EQUAL などのマクロを使用して、動作結果が期待どおりであるかどうかを確認します。すべてのテストに合格すると、プログラム全体が 0 を返し、テストが成功したことを示します。いずれかのテストが失敗した場合、プログラムは対応するエラー メッセージを出力します。これは、開発者がコードの正確さと安定性を確保するのに役立つ一般的な単体テスト戦略です。

se3 テスト ソース コード:

#include <iostream>// 包含标准输入输出流库


#include <sophus/se3.hpp>// 包含Sophus库中的SE3类
#include "tests.hpp" // 包含测试头文件


// Explicit instantiate all class templates so that all member methods
// get compiled and for code coverage analysis.
// 显式实例化所有类模板，以便所有成员方法都被编译并进行代码覆盖率分析。
namespace Eigen {
template class Map<Sophus::SE3<double>>;// 实例化Map类模板，用于映射Sophus::SE3<double>类型
template class Map<Sophus::SE3<double> const>;// 实例化Map类模板，用于映射Sophus::SE3<double> const类型
}  // namespace Eigen


namespace Sophus {


template class SE3<double, Eigen::AutoAlign>;// 实例化SE3类模板，使用double类型和Eigen::AutoAlign对齐选项
template class SE3<float, Eigen::DontAlign>;// 实例化SE3类模板，使用float类型和Eigen::DontAlign对齐选项
#if SOPHUS_CERES
template class SE3<ceres::Jet<double, 3>>;  // 如果定义了SOPHUS_CERES，则实例化SE3类模板，使用ceres::Jet<double,
                                            // 3>类型
#endif


template <class Scalar>
class Tests {// 定义Tests类模板，用于测试
 public:
  using SE3Type = SE3<Scalar>;// 定义SE3Type类型别名，等价于SE3<Scalar>
  using SO3Type = SO3<Scalar>;// 定义SO3Type类型别名，等价于SO3<Scalar>
  using Point = typename SE3<Scalar>::Point;// 定义Point类型别名，等价于SE3<Scalar>::Point
  using Tangent = typename SE3<Scalar>::Tangent;// 定义Tangent类型别名，等价于SE3<Scalar>::Tangent
  Scalar const kPi = Constants<Scalar>::pi();// 定义常量kPi，等于pi的值


  Tests() {// 构造函数
    se3_vec_ = getTestSE3s<Scalar>();// 获取测试用的SE3对象向量


    Tangent tmp;// 定义临时变量tmp
    tmp << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0);// 给tmp赋值
    tangent_vec_.push_back(tmp);// 将tmp添加到tangent_vec_向量中
    tmp << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0);
    tangent_vec_.push_back(tmp);
    tmp << Scalar(0), Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0);
    tangent_vec_.push_back(tmp);
    tmp << Scalar(0), Scalar(-5), Scalar(10), Scalar(0), Scalar(0), Scalar(0);
    tangent_vec_.push_back(tmp);
    tmp << Scalar(-1), Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(1);
    tangent_vec_.push_back(tmp);
    tmp << Scalar(20), Scalar(-1), Scalar(0), Scalar(-1), Scalar(1), Scalar(0);
    tangent_vec_.push_back(tmp);
    tmp << Scalar(30), Scalar(5), Scalar(-1), Scalar(20), Scalar(-1), Scalar(0);
    tangent_vec_.push_back(tmp);


    point_vec_.push_back(Point(Scalar(1), Scalar(2), Scalar(4))); // 添加一个点到point_vec_向量中
    point_vec_.push_back(Point(Scalar(1), Scalar(-3), Scalar(0.5)));// 添加一个点到point_vec_向量中
    point_vec_.push_back(Point(Scalar(-5), Scalar(-6), Scalar(7)));// 添加一个点到point_vec_向量中
  }


  void runAll() {// 定义runAll函数，用于运行所有测试
    bool passed = testLieProperties();// 运行testLieProperties函数，测试李群性质
    passed &= testRawDataAcces();// 运行testRawDataAcces函数，测试原始数据访问
    passed &= testMutatingAccessors();// 运行testMutatingAccessors函数，测试变异访问器
    passed &= testConstructors();// 运行testConstructors函数，测试构造函数
    passed &= testFit();           // 运行testFit函数，测试拟合
    processTestResult(passed);// 处理测试结果
  }


 private:
  bool testLieProperties() {// 定义testLieProperties函数，用于测试李群性质
    // 创建LieGroupTests对象，用于测试SE3Type类型的李群性质
    LieGroupTests<SE3Type> tests(se3_vec_, tangent_vec_, point_vec_);
    return tests.doAllTestsPass();// 返回所有测试是否通过
  }


  bool testRawDataAcces() {// 定义testRawDataAcces函数，用于测试原始数据访问
    bool passed = true;// 定义passed变量，表示所有测试是否通过
    Eigen::Matrix<Scalar, 7, 1> raw;// 定义raw矩阵，用于存储原始数据
    raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),// 给raw矩阵赋值
        Scalar(2);
    Eigen::Map<SE3Type const> map_of_const_se3(raw.data());  // 创建map_of_const_se3对象，用于映射SE3Type
                                                             // const类型的数据
    // 测试map_of_const_se3对象的unit_quaternion()函数返回值的系数是否接近raw矩阵的前4个元素
    SOPHUS_TEST_APPROX(
        passed, map_of_const_se3.unit_quaternion().coeffs().eval(),
        raw.template head<4>().eval(), Constants<Scalar>::epsilon(), "");
    // 测试map_of_const_se3对象的translation()函数返回值是否接近raw矩阵的后3个元素
    SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(),
                       raw.template tail<3>().eval(),
                       Constants<Scalar>::epsilon(), "");
    // 测试map_of_const_se3对象的unit_quaternion()函数返回值的系数的数据指针是否等于raw矩阵的数据指针
    SOPHUS_TEST_EQUAL(passed,
                      map_of_const_se3.unit_quaternion().coeffs().data(),
                      raw.data(), "");
    // 测试map_of_const_se3对象的translation()函数返回值的数据指针是否等于raw矩阵的数据指针加4
    SOPHUS_TEST_EQUAL(passed, map_of_const_se3.translation().data(),
                      raw.data() + 4, "");
    Eigen::Map<SE3Type const> const_shallow_copy = map_of_const_se3;// 创建const_shallow_copy对象，浅拷贝map_of_const_se3对象
    // 测试const_shallow_copy对象的unit_quaternion()函数返回值的系数是否等于map_of_const_se3对象的unit_quaternion()函数返回值的系数
    SOPHUS_TEST_EQUAL(passed,
                      const_shallow_copy.unit_quaternion().coeffs().eval(),
                      map_of_const_se3.unit_quaternion().coeffs().eval(), "");
    // 测试const_shallow_copy对象的translation()函数返回值是否等于map_of_const_se3对象的translation()函数返回值
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
                      map_of_const_se3.translation().eval(), "");


    Eigen::Matrix<Scalar, 7, 1> raw2;// 定义raw2矩阵，用于存储原始数据
    raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),
        Scalar(1);// 给raw2矩阵赋值
    Eigen::Map<SE3Type> map_of_se3(raw.data());// 创建map_of_se3对象，用于映射SE3Type类型的数据
    Eigen::Quaternion<Scalar> quat;// 创建quat四元数对象
    quat.coeffs() = raw2.template head<4>();// 给quat四元数对象的系数赋值
    map_of_se3.setQuaternion(quat);// 设置map_of_se3对象的四元数为quat
    map_of_se3.translation() = raw2.template tail<3>();// 设置map_of_se3对象的平移向量为raw2矩阵的后3个元素
     测试map_of_se3对象的unit_quaternion()函数返回值的系数是否接近raw2矩阵的前4个元素
    SOPHUS_TEST_APPROX(passed, map_of_se3.unit_quaternion().coeffs().eval(),
                       raw2.template head<4>().eval(),
                       Constants<Scalar>::epsilon(), "");
    // 测试map_of_se3对象的translation()函数返回值是否接近raw2矩阵的后3个元素
    SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),
                       raw2.template tail<3>().eval(),
                       Constants<Scalar>::epsilon(), "");
    // 测试map_of_se3对象的unit_quaternion()函数返回值的系数的数据指针是否等于raw矩阵的数据指针
    SOPHUS_TEST_EQUAL(passed, map_of_se3.unit_quaternion().coeffs().data(),
                      raw.data(), "");
    // 测试map_of_se3对象的translation()函数返回值的数据指针是否等于raw矩阵的数据指针加4
    SOPHUS_TEST_EQUAL(passed, map_of_se3.translation().data(), raw.data() + 4,
                      "");
    // 测试map_of_se3对象的unit_quaternion()函数返回值的系数的数据指针是否不等于quat四元数对象的系数的数据指针
    SOPHUS_TEST_NEQ(passed, map_of_se3.unit_quaternion().coeffs().data(),
                    quat.coeffs().data(), "");
    Eigen::Map<SE3Type> shallow_copy = map_of_se3;// 创建shallow_copy对象，浅拷贝map_of_se3对象
    // 测试shallow_copy对象的unit_quaternion()函数返回值的系数是否等于map_of_se3对象的unit_quaternion()函数返回值的系数
    SOPHUS_TEST_EQUAL(passed, shallow_copy.unit_quaternion().coeffs().eval(),
                      map_of_se3.unit_quaternion().coeffs().eval(), "");
    // 测试shallow_copy对象的translation()函数返回值是否等于map_of_se3对象的translation()函数返回值
    SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
                      map_of_se3.translation().eval(), "");
    Eigen::Map<SE3Type> const const_map_of_se3 = map_of_se3;// 创建const_map_of_se3对象，浅拷贝map_of_se3对象
    // 测试const_map_of_se3对象的unit_quaternion()函数返回值的系数是否等于map_of_se3对象的unit_quaternion()函数返回值的系数
    SOPHUS_TEST_EQUAL(passed,
                      const_map_of_se3.unit_quaternion().coeffs().eval(),
                      map_of_se3.unit_quaternion().coeffs().eval(), "");
    // 测试const_map_of_se3对象的translation()函数返回值是否等于map_of_se3对象的translation()函数返回值
    SOPHUS_TEST_EQUAL(passed, const_map_of_se3.translation().eval(),
                      map_of_se3.translation().eval(), "");
    // 创建const_se3常量对象，使用quat四元数和raw2矩阵的后三个元素初始化
    SE3Type const const_se3(quat, raw2.template tail<3>().eval());
    for (int i = 0; i < 7; ++i) {  // 循环遍历0到6
        // 测试const_se3常量对象的data()函数返回值第i个元素是否等于raw2矩阵第i个元素
      SOPHUS_TEST_EQUAL(passed, const_se3.data()[i], raw2.data()[i], "");
    }
    // 创建se3变量，使用quat四元数和raw2矩阵的后三个元素初始化
    SE3Type se3(quat, raw2.template tail<3>().eval());
    for (int i = 0; i < 7; ++i) {// 循环遍历0到6
        //  测试se3变量的data()函数返回值第i个元素是否等于raw2矩阵第i个元素
      SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i], "");
    }


    for (int i = 0; i < 7; ++i) {  // 循环遍历0到6
      // 测试se3变量的data()函数返回值第i个元素是否等于raw矩阵第i个元素
      SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i], "");
    }
    SE3Type trans = SE3Type::transX(Scalar(0.2));// 创建trans变量，使用SE3Type::transX函数初始化，沿x轴平移0.2
    SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2),
                       Constants<Scalar>::epsilon(), "");// 测试trans变量的translation()函数返回值的x分量是否接近0.2
    trans = SE3Type::transY(Scalar(0.7));// 使用SE3Type::transY函数初始化trans变量，沿y轴平移0.7
    SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7),
                       Constants<Scalar>::epsilon(), "");// 测试trans变量的translation()函数返回值的y分量是否接近0.7
    trans = SE3Type::transZ(Scalar(-0.2));// 使用SE3Type::transZ函数初始化trans变量，沿z轴平移-0.2
    SOPHUS_TEST_APPROX(passed, trans.translation().z(), Scalar(-0.2),
                       Constants<Scalar>::epsilon(), "");// 测试trans变量的translation()函数返回值的z分量是否接近-0.2
    Tangent t;// 定义t变量，用于存储切向量
    t << Scalar(0), Scalar(0), Scalar(0), Scalar(0.2), Scalar(0), Scalar(0);// 给t变量赋值  运动螺旋[dx,dy,dz,wx,wy,wz]
    // 测试SE3Type::rotX函数返回值的矩阵是否等于SE3Type::exp函数返回值的矩阵
    SOPHUS_TEST_EQUAL(passed, SE3Type::rotX(Scalar(0.2)).matrix(),
                      SE3Type::exp(t).matrix(), "");
    t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.2), Scalar(0); // 给t变量赋值
    SOPHUS_TEST_EQUAL(passed, SE3Type::rotY(Scalar(-0.2)).matrix(),
                      SE3Type::exp(t).matrix(), "");// 测试SE3Type::rotY函数返回值的矩阵是否等于SE3Type::exp函数返回值的矩阵
    t << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1.1);
    SOPHUS_TEST_EQUAL(passed, SE3Type::rotZ(Scalar(1.1)).matrix(),
                      SE3Type::exp(t).matrix(), "");// 测试SE3Type::rotZ函数返回值的矩阵是否等于SE3Type::exp函数返回值的矩阵


    Eigen::Matrix<Scalar, 7, 1> data1, data2;// 定义data1和data2矩阵，用于存储数据
    data1 << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(2),
        Scalar(3);// 给data1矩阵赋值
    data1 << Scalar(0), Scalar(0), Scalar(1), Scalar(0), Scalar(3), Scalar(2),
        Scalar(1);  // 给data1矩阵赋值


    Eigen::Map<SE3Type> map1(data1.data()), map2(data2.data());// 创建map1和map2对象，分别映射data1和data2矩阵的数据


    // map -> map assignment
    map2 = map1;// 将map1对象赋值给map2对象
    // 测试map1对象的matrix()函数返回值是否等于map2对象的matrix()函数返回值
    SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix(), "");


    // map -> type assignment
    SE3Type copy;// 创建copy变量
    copy = map1;// 将map1对象赋值给copy变量
    // 测试map1对象的matrix()函数返回值是否等于copy变量的matrix()函数返回值
    SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix(), "");


    // type -> map assignment
    // 使用SE3Type::trans和SE3Type::rotZ函数初始化copy变量，沿x、y、z轴分别平移4、5、6，绕z轴旋转0.5
    copy = SE3Type::trans(Scalar(4), Scalar(5), Scalar(6)) *
           SE3Type::rotZ(Scalar(0.5));
    map1 = copy;// 将copy变量赋值给map1对象
    // 测试map1对象的matrix()函数返回值是否等于copy变量的matrix()函数返回值
    SOPHUS_TEST_EQUAL(passed, map1.matrix(), copy.matrix(), "");


    return passed;  // 返回所有测试是否通过
  }
  // 定义testMutatingAccessors函数，用于测试变异访问器
  bool testMutatingAccessors() {
    bool passed = true;// 定义passed变量，表示所有测试是否通过
    SE3Type se3;// 创建se3变量
    // 创建R变量，使用SO3Type::exp函数初始化，指数映射点 旋转矢量(0.2, 0.5, 0.0)
    SO3Type R(SO3Type::exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))));
    se3.setRotationMatrix(R.matrix());  // 设置se3变量的旋转矩阵为R变量的矩阵
    // 测试se3变量的rotationMatrix()函数返回值是否接近R变量的矩阵
    SOPHUS_TEST_APPROX(passed, se3.rotationMatrix(), R.matrix(),
                       Constants<Scalar>::epsilon(), "");


    return passed;// 返回所有测试是否通过
  }
  // 定义testConstructors函数，用于测试构造函数
  bool testConstructors() {
    bool passed = true;// 定义passed变量，表示所有测试是否通过
    // 创建I矩阵，初始化为单位矩阵
    Eigen::Matrix<Scalar, 4, 4> I = Eigen::Matrix<Scalar, 4, 4>::Identity();
    // 测试SE3Type默认构造函数返回值的矩阵是否等于I矩阵
    SOPHUS_TEST_EQUAL(passed, SE3Type().matrix(), I, "");


    SE3Type se3 = se3_vec_.front();//  获取se3_vec_向量的第一个元素赋值给se3变量
    Point translation = se3.translation();// 获取se3变量的translation()函数返回值赋值给translation变量
    SO3Type so3 = se3.so3();// 获取se3变量的so3()函数返回值赋值给so3变量
    // 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
    SOPHUS_TEST_APPROX(passed, SE3Type(so3, translation).matrix(), se3.matrix(),
                       Constants<Scalar>::epsilon(), "");
    // 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
    SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(),
                       se3.matrix(), Constants<Scalar>::epsilon(), "");
    // 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
    SOPHUS_TEST_APPROX(passed,
                       SE3Type(so3.unit_quaternion(), translation).matrix(),
                       se3.matrix(), Constants<Scalar>::epsilon(), "");
    // 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
    SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(),
                       Constants<Scalar>::epsilon(), "");


    return passed;// 返回所有测试是否通过
  }


  template <class S = Scalar>
  enable_if_t<std::is_floating_point<S>::value, bool> testFit() {// 定义testFit函数模板，用于测试拟合，仅当S类型为浮点数类型时启用
    bool passed = true;// 定义passed变量，表示所有测试是否通过


    for (int i = 0; i < 100; ++i) {// 循环遍历0到99
      Matrix4<Scalar> T = Matrix4<Scalar>::Random();// 创建T矩阵，初始化为随机数
      //使用一些数值方法来计算一个最接近 T 的刚体变换矩阵。
      //这个刚体变换矩阵可以由一个旋转矩阵和一个平移向量构成，
      //这两个部分都可以从 T 矩阵中提取出来。然后，这个函数会使用这些信息来构造一个新的 SE3Type 对象并返回
      SE3Type se3 = SE3Type::fitToSE3(T);// 使用SE3Type::fitToSE3函数拟合T矩阵，赋值给se3变量
      SE3Type se3_2 = SE3Type::fitToSE3(se3.matrix());// 使用SE3Type::fitToSE3函数拟合se3变量的矩阵，赋值给se3_2变量
      // 测试se3和se2_2两个变量的矩阵是否接近
      SOPHUS_TEST_APPROX(passed, se3.matrix(), se3_2.matrix(),
                         Constants<Scalar>::epsilon(), "");
    }
    for (Scalar const angle :
         {Scalar(0.0), Scalar(0.1), Scalar(0.3), Scalar(-0.7)}) {// 循环遍历角度数组中的每个元素
      // 测试SE3Type::rotX函数返回值的angleX()函数返回值是否接近angle  
      SOPHUS_TEST_APPROX(passed, SE3Type::rotX(angle).angleX(), angle,
                         Constants<Scalar>::epsilon(), "");
      // 测试SE3Type::rotY函数返回值的angleY()函数返回值是否接近angle
      SOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle,
                         Constants<Scalar>::epsilon(), "");
      // 测试SE3Type::rotZ函数返回值的angleZ()函数返回值是否接近angle
      SOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle,
                         Constants<Scalar>::epsilon(), "");
    }
    return passed;// 返回所有测试是否通过
  }


  template <class S = Scalar>
  enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {// 定义testFit函数模板，用于测试拟合，仅当S类型不为浮点数类型时启用
    return true;// 直接返回true
  }


  std::vector<SE3Type, Eigen::aligned_allocator<SE3Type>> se3_vec_;// 定义se3_vec_向量，用于存储SE3Type类型的对象
  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;// 定义tangent_vec_向量，用于存储Tangent类型的对象
  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;// 定义point_vec_向量，用于存储Point类型的对象
};


int test_se3() {// 定义test_se3函数，用于测试SE3类
  using std::cerr;
  using std::endl;


  cerr << "Test SE3" << endl << endl;
  cerr << "Double tests: " << endl;
  Tests<double>().runAll();// 运行所有double类型的测试
  cerr << "Float tests: " << endl;
  Tests<float>().runAll();// 运行所有float类型的测试


#if SOPHUS_CERES
  cerr << "ceres::Jet<double, 3> tests: " << endl;
  Tests<ceres::Jet<double, 3>>().runAll();// 运行所有ceres::Jet<double, 3>类型的测试
#endif


  return 0;
}
}  // namespace Sophus


int main() { return Sophus::test_se3(); }// 主函数，运行Sophus命名空间下的test_se3函数，并返回其返回值

コードをテストする目的:

1. テストコードは主に、sophus ライブラリのアルゴリズム実装が正しいかどうかを検証するために使用されます。開発者はテスト ケースを作成して、さまざまなアルゴリズム シナリオの出力結果が期待と一致していることを確認します。

2. テストコードは回帰テストにも使用できます。sophus ライブラリが更新されると、テスト コードを実行して、更新されたアルゴリズムの出力が以前のアルゴリズムと一致しているかどうかを検証して、新しいバグの発生を防ぐことができます。

3. テストコードはsophusライブラリを利用したサンプルコードとして利用できます。テスト コードを表示することで、ユーザーは sophus ライブラリのインターフェイスを正しく使用する方法をより迅速に理解できます。

4. テスト コードを書くことは、コーディングの良い練習になります。適切なテストにより、コードの品質が向上し、エラーの可能性が低減されます。テスト コードを保守すると、開発者は自分が作成したコードをより深く理解できるようになります。

5. オープンソース プロジェクトはテスト コードを提供します。これにより、ユーザーはコードの品質に自信を持ち、他の開発者がプロ​​ジェクトの新しいテスト ケースを作成しやすくなります。

6. 自動テスト フレームワークは、テスト コードを実行することにより、開発者による継続的な統合と展開にも役立ちます。

ソフォス対マニフ：

参考URL：
1. https://juejin.cn/post/7067858653339975688

Sophus ソース コードの行ごとの解釈 (ビジュアル SLAM に関する第 14 回講義の概念と組み合わせた)

2. https://juejin.cn/post/7075761513113321479 

リー群の基礎