Sophusexp函数Sophus::SO3d::exp只能传入向量，传入矩阵报错 Vector3d w(0.01, 0.02, 0.03); Sop...
Sophus库通常用于 2D和 3D的几何问题(即计算机视觉、机器人应用领域的使用),库中实现了特殊正交群 S O ( 2 ) SO(2) SO(2) 和 S O ( 3 ) SO(3) SO(3) 来表示2D和3D中的旋转,以及特殊的欧几里得群 S E ( 2 ) SE(2) SE(2)和 S E ( 3 ) SE(3) SE(3)来表示2D和3D中的刚体变换。
Sophus库是一个较好的李代数库,不仅支持SO(3)和SE(3),此外还含有二维运动SO(2),SE(2)及相似变换Sim(3)的内容。它是直接在Eigen基础上开发的,不需要安装额外的依赖库。可以直接从GitHub上获取Sophus,编译即可。 下面演示Sophus库中的SO(3)和SE(3)运算: #include<iostream>#include<cmath>#include<Eigen/Co...
SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R;cout<<"SO3 updated = \n"<< SO3_updated.matrix() <<endl;cout<<"***"<<endl;// 对SE(3)操作大同小异Vector3dt(1,0,0);// 沿X轴平移1Sophus::SE3SE3_Rt(R, t);// 从R,t构造SE(3)Sophus...
std::cout << so3_Rv.transpose() <<std::endl;// 5.增加扰动Eigen::Vector3dupdate_so3(1e-4,0,0);//增加扰动Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*SO3_R; std::cout<<SO3_updated.matrix()<<std::endl;
vee为反对称到向量cout<<"so3 hat vee="<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl;//transpose纯粹是为了输出美观一些//增量扰动模型的更新Eigen::Vector3d update_so3(1e-4,0,0);//假设更新量为这么多Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3...
cout << "so3 hat vee= " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl; // 增量扰动模型的更新 Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多 Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R; ...
Sophus内部SO3部分由Eigen::Quaternion表示,translation用Eigen::Vector3d表示。通过SE3::data()可以获得SE3存储的数据的double*指针,存储地址顺序如下: #pragmaonce #include"Thirdparty/sophus/se3.hpp"#include<ceres/local_parameterization.h> classLocalParameterizationSE3:publiccer...
transpose()<<endl; Eigen::Vector3d update_so3(1e-4,0,0); //Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_1;//不会生成带^ Sophus::SO3 SO3_updated = Sophus::SO3::exp(Sophus::SO3::hat(update_so3))*SO3_1;//这样带上^再做指数映射也是可以的。 cout<<"SO...
欧拉角、旋转向量、四元数和旋转矩阵是Sophus中常常提到的几个名词,欧拉角和旋转向量是类似的,SO(3)的旋转矩阵有9个量,但是只有3个自由度,并且是单位正交矩阵,具有冗余性,对其估计或优化问题的求解不方便。我们可以用一个旋转轴和一个旋转角描述任意旋转。一个方向与旋转轴一致,长度(模)等于旋转角的向量,我们称之...