【李群李代数】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 Eigennamespace 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>类型
#endiftemplate <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>::Pointusing Tangent = typename SE3<Scalar>::Tangent;// 定义Tangent类型别名，等价于SE3<Scalar>::TangentScalar const kPi = Constants<Scalar>::pi();// 定义常量kPi，等于pi的值Tests() {// 构造函数se3_vec_ = getTestSE3s<Scalar>();// 获取测试用的SE3对象向量Tangent tmp;// 定义临时变量tmptmp << 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矩阵的数据指针加4SOPHUS_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对象的四元数为quatmap_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矩阵的数据指针加4SOPHUS_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.2SOPHUS_TEST_APPROX(passed, trans.translation().x(), Scalar(0.2),Constants<Scalar>::epsilon(), "");// 测试trans变量的translation()函数返回值的x分量是否接近0.2trans = SE3Type::transY(Scalar(0.7));// 使用SE3Type::transY函数初始化trans变量，沿y轴平移0.7SOPHUS_TEST_APPROX(passed, trans.translation().y(), Scalar(0.7),Constants<Scalar>::epsilon(), "");// 测试trans变量的translation()函数返回值的y分量是否接近0.7trans = SE3Type::transZ(Scalar(-0.2));// 使用SE3Type::transZ函数初始化trans变量，沿z轴平移-0.2SOPHUS_TEST_APPROX(passed, trans.translation().z(), Scalar(-0.2),Constants<Scalar>::epsilon(), "");// 测试trans变量的translation()函数返回值的z分量是否接近-0.2Tangent 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 assignmentmap2 = map1;// 将map1对象赋值给map2对象// 测试map1对象的matrix()函数返回值是否等于map2对象的matrix()函数返回值SOPHUS_TEST_EQUAL(passed, map1.matrix(), map2.matrix(), "");// map -> type assignmentSE3Type 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.5copy = 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到99Matrix4<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()函数返回值是否接近angleSOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle,Constants<Scalar>::epsilon(), "");// 测试SE3Type::rotZ函数返回值的angleZ()函数返回值是否接近angleSOPHUS_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_CEREScerr << "ceres::Jet<double, 3> tests: " << endl;Tests<ceres::Jet<double, 3>>().runAll();// 运行所有ceres::Jet<double, 3>类型的测试
#endifreturn 0;
}
}  // namespace Sophusint main() { return Sophus::test_se3(); }// 主函数，运行Sophus命名空间下的test_se3函数，并返回其返回值

测试代码的目的：