Sophus初始化SE3d，旋转矩阵不正交

最近使用Sophus库中的SE3d类，初始化报错，报错说R is not orthogonal，传入的旋转矩阵不正交。报错的代码如下：

void AbsoluteTrajectoryError(
        const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> & gt_trajectery,
        const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> & esti_trajectery,
        double & error_value
)
{
    error_value = 0;
    if(gt_trajectery.size()!=esti_trajectery.size()){
        error_value=-1;
        return;
    }

    Eigen::Matrix3d rotation = gt_trajectery[0].rotation();

    std::cout<< rotation*rotation.transpose()<<std::endl;
    std::cout<< gt_trajectery[0].rotation()<<std::endl;

    std::cout<<"to be "<<std::endl;
    std::cout<<gt_trajectery[0].rotation()<<std::endl;
    for(int i=0;i<gt_trajectery.size();i++){
        Sophus::SE3d gt_i(gt_trajectery[i].rotation(), gt_trajectery[i].translation());
        Sophus::SE3d esti_i(esti_trajectery[i].rotation(), esti_trajectery[i].translation());
    }

    return;
}

这段代码中，旋转矩阵相乘后的输出是一个精度不高的单位矩阵（就是对角线元素是1，然后其他元素是极小量

点进去看是Sophus中有一个isOrthogoal的检查，没有通过Sophus的精度检查。解决方法是先将旋转矩阵转变成四元数，然后归一化，再转成旋转矩阵，这样旋转矩阵能够通过正交检查，虽然有点多此一举…但是挺有用的。SLAM过程中的数值精度也非常重要，毕竟矩阵连乘，一点点数值误差会造成大量偏移。
代码如下：

Eigen::Quaterniond rotation(gt_trajectery[0].rotation());
rotation.normalize();
Sophus::SE3d gt_i(rotation.toRotationMatrix(), gt_trajectery[i].translation());