sophus库遇到的问题

问题报错

系统环境

ubuntu20.
sophus版本具体未知，是r3live直接在tools目录中拷贝的一份sophus源码，然后cpp文件直接include。

报错信息

Sophus ensure failed in function 'Sophus::SO3<Scalar_, Options>::SO3
(const Transformation&) [with Scalar_ = double; int Options = 0; Sophus::SO3
<Scalar_, Options>::Transformation = Eigen::Matrix<double, 3, 3>]', file
 '/home/xz/code/visual_lidar_slam/r3live_velodyne_mapbased/src/r3live_noted-
 master/r3live/./src/tools/lib_sophus/so3.hpp', line 472.
R is not orthogonal:
            1 -4.89682e-11 -1.01423e-10
-4.89682e-11            1  -4.0662e-12
-1.01423e-10  -4.0662e-12            1

看信息应该是给一个so3赋值的时候,3*3的旋转矩阵并不是正交矩阵 ,所以报错.

问题解决方法

在声明so3d的时候需要将旋转矩阵正交化后再给so3d初始化。
源代码：

Sophus::SO3d reg_so3 = Sophus::SO3d(reg_pose.block<3,3>(0,0));

修改后：

Sophus::SO3d reg_so3 = Sophus::SO3d(Eigen::Quaterniond(reg_pose.block<3,3>
(0,0)).normalized().toRotationMatrix());

就是先转成四元数正交化后再转回旋转矩阵。