一、Sophus库简介:
Eigen库是一个开源的C++线性代数库,它提供了快速的有关矩阵的线性代数运算,还包括解方程等功能。但是Eigen库提供了集合模块,但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库,它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的,继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时,既可以使用Eigen命名空间,也可以使用Sophus命名空间。
Eigen::Matrix3d和Sophus::Matrix3d
Eigen::Vector3d和Sophus::Vector3d
此外,为了方便说明SE(4)和se(4),Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等,即:
Sophus::Vector4d
Sophus::Matrix4d
Sophus::Vector6d
Sophus::Matrix6d
二、Sophus的基本使用
笔者使用的是带模板的Sophus库,本库使用起来也很简单。其中李代数下的SO3的类为
Sophus::SO3d ,可以直接使用Eigen中的旋转矩阵和四元数进行初始化。
AngleAxisd roation_vector(M_PI/2,Vector3d(0,0,1));
Eigen::Matrix3d R=roation_vector.toRotationMatrix();
Quaterniond q(R);
Sophus::SO3d so3_R(R);
Sophus::SO3d so3_Q(q);
了解李群李代数的都知道SO3上还是表示矩阵,我们初始化的矩阵如何进行输出?需要.matrix()输出矩阵。
cout<<"this from matrix : \n"<<so3_R.matrix()<<endl;
cout<<"this from quater : \n"<<so3_Q.matrix()<<endl;
1、使用对数映射转换为so3
SO3.log()就可以将SO3上的元素对数表示到so3中,其中so3的定义依然是Vector3d,其实就是Eigen中的类。
Vector3d so3=so3_Q.log();
cout<<"so3 = \n"<<so3.transpose()<<endl;
2、so3->SO3
使用SO3d::hat()可以实现so3->SO3的转换。
//将so3转换到SO3中
cout<<"so3 hat= \n"<<Sophus::SO3d::hat(so3)<<end
使用so3d::vee可以实现SO3->so3的转换:
//SO3转换到so3中
cout<<"SO3 vee=\n"<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl
3、增加扰动
学过slam十四讲的应该知道,旋转矩阵不满足加法的封闭,这样求导就很麻烦。所以引了BCH近似公式来近似加法。Sophus中提供exp用来进行计算:
Vector3d update_so3(1e-4,0,0);//增加扰动
Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*so3_R;
cout<<"so3 update =\n"<<SO3_updated.matrix()<<endl;
学完so3和SO3接下的SE3就很简单了是大同小异的,所以直接贴代码很容易就可以看懂。
4、SE3和se3
//se3和SE3
Vector3d t(1,0,0);
Sophus::SE3d SE3_Rt(R,t);
Sophus::SE3d SE3_qt(q,t);
cout<<"this from matrix : \n"<<SE3_Rt.matrix()<<endl;
cout<<"this from quater : \n"<<SE3_qt.matrix()<<endl;
//因为se3为6维的向量,而Eigen中没有6为vecor6d,所以自己定义一个
typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3=SE3_Rt.log();
cout<<"se3= \n"<<se3.transpose()<<endl; //平移在前而旋转在后
//同样使用exp和vee
//将se3转换到SE3中
cout<<"se3 hat= \n"<<Sophus::SE3d::hat(se3)<<endl;
//SE3转换到se3中
cout<<"SE3 vee=\n"<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;
//同样加入扰动
Vector6d update_se3;
update_se3.setZero();
update_se3(1,0)=1e-4d;
cout<<"update_se3 is \n"<<update_se3.transpose()<<endl;
Sophus::SE3d SE3_updated=Sophus::SE3d::exp(update_se3)*SE3_Rt;
cout<<" SE3 update is \n"<<SE3_updated.matrix()<<endl;
以下为so3到SO3转换和SE3到se3的转换图:
文章来源:https://www.toymoban.com/news/detail-408158.html
文章来源地址https://www.toymoban.com/news/detail-408158.html
到了这里,关于视觉slam中Sophus库的基本使用的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!