【李群李代数】Sophus库中SE3类测试（附manif 与sophus 对比）
测试演示
测试结果
对Sophus库中SE3类进行一系列的测试，包括李群性质、原始数据访问、变异访问器、构造函数以及拟合等方面。在每个测试中，都会使用一些预设的数据进行操作，并通过SOPHUS_TEST_APPROX和SOPHUS_TEST_EQUAL等宏来检查操作结果是否符合预期。如果所有测试都通过，则整个程序会返回0，表示测试成功。如果有任何一个测试未通过，则程序会输出相应的错误信息。这是一种常见的单元测试策略，可以帮助开发者确保代码的正确性和稳定性。
se3测试源码：
#include // 包含标准输入输出流库
#include // 包含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>;// 实例化Map类模板，用于映射Sophus::SE3类型
template class Map const>;// 实例化Map类模板，用于映射Sophus::SE3 const类型
} // namespace Eigen
namespace Sophus {
template class SE3;// 实例化SE3类模板，使用double类型和Eigen::AutoAlign对齐选项
template class SE3;// 实例化SE3类模板，使用float类型和Eigen::DontAlign对齐选项
#if SOPHUS_CERES
template class SE3>; // 如果定义了SOPHUS_CERES，则实例化SE3类模板，使用ceres::Jet类型
#endif
template
class Tests {// 定义Tests类模板，用于测试
public:
using SE3Type = SE3;// 定义SE3Type类型别名，等价于SE3
using SO3Type = SO3;// 定义SO3Type类型别名，等价于SO3
using Point = typename SE3::Point;// 定义Point类型别名，等价于SE3::Point
using Tangent = typename SE3::Tangent;// 定义Tangent类型别名，等价于SE3::Tangent
Scalar const kPi = Constants::pi();// 定义常量kPi，等于pi的值
Tests() {// 构造函数
se3_vec_ = getTestSE3s();// 获取测试用的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 tests(se3_vec_, tangent_vec_, point_vec_);
return tests.doAllTestsPass();// 返回所有测试是否通过
}
bool testRawDataAcces() {// 定义testRawDataAcces函数，用于测试原始数据访问
bool passed = true;// 定义passed变量，表示所有测试是否通过
Eigen::Matrix raw;// 定义raw矩阵，用于存储原始数据
raw << Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),// 给raw矩阵赋值
Scalar(2);
Eigen::Map 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::epsilon(), "");
// 测试map_of_const_se3对象的translation()函数返回值是否接近raw矩阵的后3个元素
SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(),
raw.template tail<3>().eval(),
Constants::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 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 raw2;// 定义raw2矩阵，用于存储原始数据
raw2 << Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),
Scalar(1);// 给raw2矩阵赋值
Eigen::Map map_of_se3(raw.data());// 创建map_of_se3对象，用于映射SE3Type类型的数据
Eigen::Quaternion 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::epsilon(), "");
// 测试map_of_se3对象的translation()函数返回值是否接近raw2矩阵的后3个元素
SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),
raw2.template tail<3>().eval(),
Constants::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 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 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::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::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::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 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 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::epsilon(), "");
return passed;// 返回所有测试是否通过
}
// 定义testConstructors函数，用于测试构造函数
bool testConstructors() {
bool passed = true;// 定义passed变量，表示所有测试是否通过
// 创建I矩阵，初始化为单位矩阵
Eigen::Matrix I = Eigen::Matrix::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::epsilon(), "");
// 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(),
se3.matrix(), Constants::epsilon(), "");
// 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
SOPHUS_TEST_APPROX(passed,
SE3Type(so3.unit_quaternion(), translation).matrix(),
se3.matrix(), Constants::epsilon(), "");
// 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵
SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(),
Constants::epsilon(), "");
return passed;// 返回所有测试是否通过
}
template
enable_if_t::value, bool> testFit() {// 定义testFit函数模板，用于测试拟合，仅当S类型为浮点数类型时启用
bool passed = true;// 定义passed变量，表示所有测试是否通过
for (int i = 0; i < 100; ++i) {// 循环遍历0到99
Matrix4 T = Matrix4::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::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::epsilon(), "");
// 测试SE3Type::rotY函数返回值的angleY()函数返回值是否接近angle
SOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle,
Constants::epsilon(), "");
// 测试SE3Type::rotZ函数返回值的angleZ()函数返回值是否接近angle
SOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle,
Constants::epsilon(), "");
}
return passed;// 返回所有测试是否通过
}
template
enable_if_t::value, bool> testFit() {// 定义testFit函数模板，用于测试拟合，仅当S类型不为浮点数类型时启用
return true;// 直接返回true
}
std::vector> se3_vec_;// 定义se3_vec_向量，用于存储SE3Type类型的对象
std::vector> tangent_vec_;// 定义tangent_vec_向量，用于存储Tangent类型的对象
std::vector> 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().runAll();// 运行所有double类型的测试
cerr << "Float tests: " << endl;
Tests().runAll();// 运行所有float类型的测试
#if SOPHUS_CERES
cerr << "ceres::Jet tests: " << endl;
Tests>().runAll();// 运行所有ceres::Jet类型的测试
#endif
return 0;
}
} // namespace Sophus
int main() { return Sophus::test_se3(); }// 主函数，运行Sophus命名空间下的test_se3函数，并返回其返回值 测试代码的目的：
测试代码主要是为了验证sophus库中的算法实现是否正确。开发者通过编写测试用例,来确保各种算法场景下的输出结果与预期一致。
测试代码也可以用于回归测试。当sophus库有更新时,可以运行测试代码,验证更新后算法的输出是否与之前一致,防止引入新的bug。
测试代码可以作为使用sophus库的示例代码。通过查看测试代码,用户可以更快地理解如何正确使用sophus库中的接口。
编写测试代码是良好的编码习惯。充分的测试可以提高代码质量,降低出错概率。维护测试代码也可以让开发者更好地理解自己编写的代码。
开源项目提供测试代码,可以让用户对代码质量更有信心,也方便其他开发者为项目编写新的测试用例。
自动化测试框架也可以通过运行测试代码,帮助开发者进行持续集成和部署。
sophus vs manif :
参考网址：
1. https://juejin.cn/post/7067858653339975688
Sophus源码逐行解读 ( 结合视觉SLAM十四讲的概念 )
2. https://juejin.cn/post/7075761513113321479
Lie Group Foundations