最近使用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());