test demo
Test Results
Conduct a series of tests on the SE3 class in the Sophus library, including Lie group properties, original data access, mutation accessors, constructors, and fitting. In each test, some preset data will be used for operation, and macros such as SOPHUS_TEST_APPROX and SOPHUS_TEST_EQUAL will be used to check whether the operation results are as expected. If all tests pass, the entire program returns 0, indicating that the test was successful. If any test fails, the program will output the corresponding error message. This is a common unit testing strategy that can help developers ensure the correctness and stability of their code.
se3 test source code:
#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函数，并返回其返回值Purpose of testing code:
The test code is mainly to verify whether the algorithm implementation in the sophus library is correct. Developers write test cases to ensure that the output results in various algorithm scenarios are consistent with expectations.
Test code can also be used for regression testing. When the sophus library is updated, you can run the test code to verify whether the output of the updated algorithm is consistent with the previous one to prevent the introduction of new bugs.
The test code can be used as sample code using the sophus library. By viewing the test code, users can more quickly understand how to correctly use the interfaces in the sophus library.
Writing test code is good coding practice. Adequate testing can improve code quality and reduce the probability of errors. Maintaining test code also allows developers to better understand the code they write.
Open source projects provide test code, which can make users more confident in the quality of the code and make it easier for other developers to write new test cases for the project.
Automated testing frameworks can also help developers with continuous integration and deployment by running test code.
sophus vs manif :
Reference URL:
1. https://juejin.cn/post/7067858653339975688
Line-by-line interpretation of Sophus source code (combined with the concepts of the fourteenth lecture on visual SLAM)
2. https://juejin.cn/post/7075761513113321479
Lie Group Foundations