首先是cmakelists:
然后一些include:
首先说明一下构造方式:
整体思路很明显,由同一个轴角,构造出旋转矩阵,四元数。再去构造SO3,理论上构造出来的应该都一样。
输出结果如下:
很明显,一样的,没毛病。从输出的形式可以看出,虽然SO3是李群,是旋转矩阵,但是输出形式还是向量(被转化成李代数输出)。
看一下源代码中重载的<<运算也很明显:
有一个.log()转化成李代数之后再输出的。
这里重点说一下这条构造,这条构造语句后面测试的时候,有坑: 这条语句。第一次看想当然以为是旋转向量的三个坐标值进行构造的。因为输出的值跟输进去的值是一样的: 假如,这条语句真的就是直接旋转向量构造,那么用(1,1,1)构造的话,输出也应该是1,1,1吧。下面测试:
输出:
嗯?什么鬼~
扒一扒源代码吧,显示此条构造是这样:
这就有点思路了,显示的貌似是三个过程,先转X轴,再转Y轴,再转Z轴,完全跟旋转向量不搭边。瞅着过程有点像欧拉角的过程,三个轴分了三步。
那么问题来了,我就有一个(1, 1, 1)旋转向量,如何构造成SO3呢?也就是让它输出(1, 1, 1)。
OK,是这么玩的:
sophus库中,压根就没有so3这么一说,就是SO3类,所以,so3的本质就是个三维向量,直接Eigen::Vector3d定义。
定义完了之后由指数映射成矩阵,赋值给SO3,这样就好了。少废话,看输出:
叵费~~
切记SO3是矩阵,是李群 。
试验继续:
代码看出来:
.log运算将李群转化成李代数。
hat运算将向量转成反对称阵。
vee运算将反对称阵转成向量。
输出:
看起来没啥毛病~
继续SE(3)操作:
输出: