测试左乘扰动 右乘扰动的区别
测试结果:
扰动量越大 左右乘的差异越明显 扰动量越小 基本没差异
//测试左乘扰动 右乘扰动的区别
Eigen::Quaterniond quart(0.9999862495139974,0.005014826896838862, 0.0008264610804482406, -0.0012919969843855261 );
Sophus::SO3d so3(quart);
Eigen::Vector3d update_so3(deltaValue, 0, 0);
Sophus::SO3<double> SO3_updated = so3 * Sophus::SO3<double>::exp(update_so3);
Sophus::SO3<double> SO3_updatedleft = Sophus::SO3<double>::exp(update_so3)*so3;
std::cout << "SO3_updated right : " << SO3_updated.log().x() << "," << SO3_updated.log().y() << "," << SO3_updated.log().z() << std::endl;
std::cout << "SO3_updatedleft : " << SO3_updatedleft.log().x() << "," << SO3_updatedleft.log().y() << "," << SO3_updatedleft.log().z() << std::endl;
Eigen::Quaterniond QuartOutright(SO3_updated.matrix());
Eigen::Quaterniond QuartOutleft(SO3_updatedleft.matrix());
std::cout << "QuartOutright : " << QuartOutright.x() << "," << QuartOutright.y() << "," << QuartOutright.z() << "," << QuartOutright.w() <<std::endl;
std::cout << "QuartOutleft : " << QuartOutleft.x() << "," << QuartOutleft.y() << "," << QuartOutleft.z() << "," << QuartOutleft.w() <<std::endl;
//结论:扰动量越大 左右乘的差异越明显 扰动量越小 基本没差异
//扰动为1e-7
//SO3_updated right : 0.0100298,0.00165293,-0.00258401
//SO3_updatedleft : 0.0100298,0.00165293,-0.00258401
//QuartOutright : 0.00501488,0.000826461,-0.001292,0.999986
//QuartOutleft : 0.00501488,0.000826461,-0.001292,0.999986
//扰动为1e-3
//SO3_updated right : 0.0110297,0.00165164,-0.00258483
//SO3_updatedleft : 0.0110297,0.00165422,-0.00258318
//QuartOutright : 0.00551482,0.000825815,-0.00129241,0.999984
//QuartOutleft : 0.00551482,0.000827107,-0.00129158,0.999984
//扰动为1e-1
/*SO3_updated right : 0.11003,0.00152248,-0.00266472
SO3_updatedleft : 0.11003,0.0017809,-0.00249941
QuartOutright : 0.054987,0.000760855,-0.00133169,0.998486
QuartOutleft : 0.054987,0.000890001,-0.00124908,0.998486*/
//扰动为1e-8
//SO3_updated right : 0.0100297,0.00165293,-0.00258401
//SO3_updatedleft : 0.0100297,0.00165293,-0.00258401
//QuartOutright : 0.00501483,0.000826461,-0.001292,0.999986
//QuartOutleft : 0.00501483,0.000826461,-0.001292,0.999986