视觉SLAM十四讲 从理论到实践-第九讲实践：设计前端，关于Sophus库中SO3类构造函数使用疑惑

在练习视觉SLAM十四讲 从理论到实践-第九讲实践：设计前端实验时，碰到了关于一处使用Sophus库中SO3类构造函数的疑惑。
Sophus库中：

SO3::SO3(double rot_x, double rot_y, double rot_z)
{
  unit_quaternion_
      = (SO3::exp(Vector3d(rot_x, 0.f, 0.f))
         *SO3::exp(Vector3d(0.f, rot_y, 0.f))
         *SO3::exp(Vector3d(0.f, 0.f, rot_z))).unit_quaternion_;
}

从该构造函数的实现来看， 该函数的参数为欧拉角，书上代码实现时，SO3的构造函数调用却用了从

 cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);

获得的旋转向量中的每个对应元素，即

T_c_r_estimated_ = SE3(
        SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)), 
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );

，这里显然跟SO3的定义实现是不符合的。起初很是疑惑，甚至想象为旋转向量可以分解为其对应3个元素的欧拉旋转矩阵连乘形式（即SO3定义中所示），但是举例如下：
旋转向量：v=(PI/2, PI/2, 0)
欧拉旋转变化：R = R(X, PI/2) * R(Y, PI/2)
经过作图可以很容易证明，二者是不等价的。且先看正确代码，以及实验结果验证。

void VisualOdometry::poseEstimationPnP()
{
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;

    for(cv::DMatch m : feature_matches_)
    {
        pts3d.push_back(pts_3d_ref_[m.queryIdx]);
        pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
    }
    Mat K = (cv::Mat_<double>(3,3) <<
             ref_->camera_->fx_, 0, ref_->camera_->cx_,
             0, ref_->camera_->fy_, ref_->camera_->cy_,
             0,0,1);
    Mat rvec, tvec, inliers;
    cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
    num_inliers_ = inliers.rows;
    cout << "pnp inliers: " << num_inliers_ << endl;
    
    
    // 此处旋转向量经罗德里格斯转换
    Mat R;
    cv::Rodrigues(rvec, R);
    Eigen::Matrix3d RE;
    RE << R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2);
    // SO3构造函数参数为旋转矩阵
    T_c_r_estimated_ = SE3(SO3(RE),
                          Vector3d(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0)));


    // 经验证证明，下式为小旋转量时的近似取值
//    T_c_r_estimated_ = SE3(SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)),
//                           Vector3d(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0)));

    // using bundle adjustment to optimize the pose
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType * linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block(linearSolver);
    g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

书上原来的代码，无BA优化的部分结果：
VO costs time: 0.03461
extract keypoints cost time: 0.006476
descriptor computation cost time: 0.006352
good matches: 415
match cost time: 0.015432
pnp inliers: 411
0.00138308 -0.00248132 0.00178398
0.000308052 0.00147007 -0.000874968

VO costs time: 0.029812
extract keypoints cost time: 0.010289
descriptor computation cost time: 0.00585
good matches: 400
match cost time: 0.016064
pnp inliers: 397
-0.000784515 -0.000247304 0.000747317
0.00129301 0.00148954 0.000108656

VO costs time: 0.033383
extract keypoints cost time: 0.00711
descriptor computation cost time: 0.005964
good matches: 398
match cost time: 0.015669
pnp inliers: 394
-0.00211969 0.00225218 0.00112379
-0.000926058 0.000652907 0.000526384

VO costs time: 0.030122
/home/liqiang/Practise/vslam/slambook/exe/project/version0.1/VslamLearn/bin/run_vo exited with code 0

书上原来的代码，经BA优化的部分结果：

VO costs time: 0.032901
extract keypoints cost time: 0.006294
descriptor computation cost time: 0.006143
good matches: 415
match cost time: 0.015853
pnp inliers: 411
0.00138529 -0.00248009 0.0017857
0.000308052 0.00147007 -0.000874968

VO costs time: 0.030218
extract keypoints cost time: 0.006495
descriptor computation cost time: 0.006036
good matches: 400
match cost time: 0.016007
pnp inliers: 397
-0.000784422 -0.000247597 0.00074722
0.00129301 0.00148954 0.000108656

VO costs time: 0.030737
extract keypoints cost time: 0.006861
descriptor computation cost time: 0.00613
good matches: 398
match cost time: 0.015736
pnp inliers: 394
-0.00212096 0.00225099 0.00112618
-0.000926058 0.000652907 0.000526384

VO costs time: 0.030701
/home/liqiang/Practise/vslam/slambook/exe/project/version0.1/VslamLearn/bin/run_vo exited with code 0

旋转向量经罗德里格斯处理得到旋转矩阵后调用SO3构造函数的结果（没有BA优化）：
VO costs time: 0.03079
extract keypoints cost time: 0.007631
descriptor computation cost time: 0.006795
good matches: 415
match cost time: 0.016033
pnp inliers: 411
0.00138529 -0.00248009 0.0017857
0.000308052 0.00147007 -0.000874968

VO costs time: 0.031775
extract keypoints cost time: 0.006933
descriptor computation cost time: 0.006023
good matches: 400
match cost time: 0.015726
pnp inliers: 397
-0.000784422 -0.000247597 0.00074722
0.00129301 0.00148954 0.000108656

VO costs time: 0.029855
extract keypoints cost time: 0.007465
descriptor computation cost time: 0.006319
good matches: 398
match cost time: 0.015806
pnp inliers: 394
-0.00212096 0.00225099 0.00112618
-0.000926058 0.000652907 0.000526384

VO costs time: 0.030947
/home/liqiang/Practise/vslam/slambook/exe/project/version0.1/VslamLearn/bin/run_vo exited with code 0

3组实验结果说明，SO3用小的旋转向量对应元素初始化时，结果与正确结果非常接近，误差很小。同时，当旋转向量经过罗德里格斯转换为旋转矩阵后初始化SO3的结果，即使没有经过BA优化，其结果却和BA优化后的结果一模一样。为此疑惑也便得解。

来源：CSDN

作者：RobotLife

链接：https://blog.csdn.net/RobotLife/article/details/85239610