网站系统设计目标,wordpress变慢,创意wordpress主题,打开上次浏览的网站测试演示 测试结果 对Sophus库中SE3类进行一系列的测试#xff0c;包括李群性质、原始数据访问、变异访问器、构造函数以及拟合等方面。在每个测试中#xff0c;都会使用一些预设的数据进行操作#xff0c;并通过SOPHUS_TEST_APPROX和SOPHUS_TEST_EQUAL等宏来检查操作结果是… 测试演示 测试结果 对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 MapSophus::SE3double;// 实例化Map类模板用于映射Sophus::SE3double类型
template class MapSophus::SE3double const;// 实例化Map类模板用于映射Sophus::SE3double const类型
} // namespace Eigennamespace Sophus {template class SE3double, Eigen::AutoAlign;// 实例化SE3类模板使用double类型和Eigen::AutoAlign对齐选项
template class SE3float, Eigen::DontAlign;// 实例化SE3类模板使用float类型和Eigen::DontAlign对齐选项
#if SOPHUS_CERES
template class SE3ceres::Jetdouble, 3; // 如果定义了SOPHUS_CERES则实例化SE3类模板使用ceres::Jetdouble,// 3类型
#endiftemplate class Scalar
class Tests {// 定义Tests类模板用于测试public:using SE3Type SE3Scalar;// 定义SE3Type类型别名等价于SE3Scalarusing SO3Type SO3Scalar;// 定义SO3Type类型别名等价于SO3Scalarusing Point typename SE3Scalar::Point;// 定义Point类型别名等价于SE3Scalar::Pointusing Tangent typename SE3Scalar::Tangent;// 定义Tangent类型别名等价于SE3Scalar::TangentScalar const kPi ConstantsScalar::pi();// 定义常量kPi等于pi的值Tests() {// 构造函数se3_vec_ getTestSE3sScalar();// 获取测试用的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类型的李群性质LieGroupTestsSE3Type tests(se3_vec_, tangent_vec_, point_vec_);return tests.doAllTestsPass();// 返回所有测试是否通过}bool testRawDataAcces() {// 定义testRawDataAcces函数用于测试原始数据访问bool passed true;// 定义passed变量表示所有测试是否通过Eigen::MatrixScalar, 7, 1 raw;// 定义raw矩阵用于存储原始数据raw Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1), Scalar(3),// 给raw矩阵赋值Scalar(2);Eigen::MapSE3Type 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 head4().eval(), ConstantsScalar::epsilon(), );// 测试map_of_const_se3对象的translation()函数返回值是否接近raw矩阵的后3个元素SOPHUS_TEST_APPROX(passed, map_of_const_se3.translation().eval(),raw.template tail3().eval(),ConstantsScalar::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::MapSE3Type 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::MatrixScalar, 7, 1 raw2;// 定义raw2矩阵用于存储原始数据raw2 Scalar(1), Scalar(0), Scalar(0), Scalar(0), Scalar(3), Scalar(2),Scalar(1);// 给raw2矩阵赋值Eigen::MapSE3Type map_of_se3(raw.data());// 创建map_of_se3对象用于映射SE3Type类型的数据Eigen::QuaternionScalar quat;// 创建quat四元数对象quat.coeffs() raw2.template head4();// 给quat四元数对象的系数赋值map_of_se3.setQuaternion(quat);// 设置map_of_se3对象的四元数为quatmap_of_se3.translation() raw2.template tail3();// 设置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 head4().eval(),ConstantsScalar::epsilon(), );// 测试map_of_se3对象的translation()函数返回值是否接近raw2矩阵的后3个元素SOPHUS_TEST_APPROX(passed, map_of_se3.translation().eval(),raw2.template tail3().eval(),ConstantsScalar::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::MapSE3Type 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::MapSE3Type 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 tail3().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 tail3().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),ConstantsScalar::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),ConstantsScalar::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),ConstantsScalar::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::MatrixScalar, 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::MapSE3Type 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(),ConstantsScalar::epsilon(), );return passed;// 返回所有测试是否通过}// 定义testConstructors函数用于测试构造函数bool testConstructors() {bool passed true;// 定义passed变量表示所有测试是否通过// 创建I矩阵初始化为单位矩阵Eigen::MatrixScalar, 4, 4 I Eigen::MatrixScalar, 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(),ConstantsScalar::epsilon(), );// 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵SOPHUS_TEST_APPROX(passed, SE3Type(so3.matrix(), translation).matrix(),se3.matrix(), ConstantsScalar::epsilon(), );// 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵SOPHUS_TEST_APPROX(passed,SE3Type(so3.unit_quaternion(), translation).matrix(),se3.matrix(), ConstantsScalar::epsilon(), );// 测试SE3Type构造函数返回值的矩阵是否接近se3变量的矩阵SOPHUS_TEST_APPROX(passed, SE3Type(se3.matrix()).matrix(), se3.matrix(),ConstantsScalar::epsilon(), );return passed;// 返回所有测试是否通过}template class S Scalarenable_if_tstd::is_floating_pointS::value, bool testFit() {// 定义testFit函数模板用于测试拟合仅当S类型为浮点数类型时启用bool passed true;// 定义passed变量表示所有测试是否通过for (int i 0; i 100; i) {// 循环遍历0到99Matrix4Scalar T Matrix4Scalar::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(),ConstantsScalar::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,ConstantsScalar::epsilon(), );// 测试SE3Type::rotY函数返回值的angleY()函数返回值是否接近angleSOPHUS_TEST_APPROX(passed, SE3Type::rotY(angle).angleY(), angle,ConstantsScalar::epsilon(), );// 测试SE3Type::rotZ函数返回值的angleZ()函数返回值是否接近angleSOPHUS_TEST_APPROX(passed, SE3Type::rotZ(angle).angleZ(), angle,ConstantsScalar::epsilon(), );}return passed;// 返回所有测试是否通过}template class S Scalarenable_if_t!std::is_floating_pointS::value, bool testFit() {// 定义testFit函数模板用于测试拟合仅当S类型不为浮点数类型时启用return true;// 直接返回true}std::vectorSE3Type, Eigen::aligned_allocatorSE3Type se3_vec_;// 定义se3_vec_向量用于存储SE3Type类型的对象std::vectorTangent, Eigen::aligned_allocatorTangent tangent_vec_;// 定义tangent_vec_向量用于存储Tangent类型的对象std::vectorPoint, Eigen::aligned_allocatorPoint 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;Testsdouble().runAll();// 运行所有double类型的测试cerr Float tests: endl;Testsfloat().runAll();// 运行所有float类型的测试#if SOPHUS_CEREScerr ceres::Jetdouble, 3 tests: endl;Testsceres::Jetdouble, 3().runAll();// 运行所有ceres::Jetdouble, 3类型的测试
#endifreturn 0;
}
} // namespace Sophusint 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
