Sophus が SE3d を初期化すると、回転行列が直交しない

最近、Sophus ライブラリの SE3d クラスを使用してエラーを初期化したところ、R が直交ではなく、受信した回転行列が直交ではないというエラーが発生しました。エラーコードは次のとおりです。

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());