視覺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優化後的結果一模一樣。爲此疑惑也便得解。