本文主要是介绍视觉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 posetypedef 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优化后的结果一模一样。为此疑惑也便得解。
这篇关于视觉SLAM十四讲 从理论到实践-第九讲实践：设计前端，关于Sophus库中SO3类构造函数使用疑惑的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
