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

本文主要是介绍视觉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类构造函数使用疑惑的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/186440

相关文章

Java Lambda表达式的使用详解

《JavaLambda表达式的使用详解》:本文主要介绍JavaLambda表达式的使用方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、前言二、Lambda表达式概述1. 什么是Lambda表达式?三、Lambda表达式的语法规则1. 无参数的Lambda表

java中Optional的核心用法和最佳实践

《java中Optional的核心用法和最佳实践》Java8中Optional用于处理可能为null的值,减少空指针异常,:本文主要介绍java中Optional核心用法和最佳实践的相关资料,文中... 目录前言1. 创建 Optional 对象1.1 常规创建方式2. 访问 Optional 中的值2.1

Spring组件实例化扩展点之InstantiationAwareBeanPostProcessor使用场景解析

《Spring组件实例化扩展点之InstantiationAwareBeanPostProcessor使用场景解析》InstantiationAwareBeanPostProcessor是Spring... 目录一、什么是InstantiationAwareBeanPostProcessor?二、核心方法解

详解如何使用Python构建从数据到文档的自动化工作流

《详解如何使用Python构建从数据到文档的自动化工作流》这篇文章将通过真实工作场景拆解,为大家展示如何用Python构建自动化工作流,让工具代替人力完成这些数字苦力活,感兴趣的小伙伴可以跟随小编一起... 目录一、Excel处理:从数据搬运工到智能分析师二、PDF处理:文档工厂的智能生产线三、邮件自动化:

Nginx Location映射规则总结归纳与最佳实践

《NginxLocation映射规则总结归纳与最佳实践》Nginx的location指令是配置请求路由的核心机制,其匹配规则直接影响请求的处理流程,下面给大家介绍NginxLocation映射规则... 目录一、Location匹配规则与优先级1. 匹配模式2. 优先级顺序3. 匹配示例二、Proxy_pa

Spring @RequestMapping 注解及使用技巧详解

《Spring@RequestMapping注解及使用技巧详解》@RequestMapping是SpringMVC中定义请求映射规则的核心注解,用于将HTTP请求映射到Controller处理方法... 目录一、核心作用二、关键参数说明三、快捷组合注解四、动态路径参数(@PathVariable)五、匹配请

Java 枚举的基本使用方法及实际使用场景

《Java枚举的基本使用方法及实际使用场景》枚举是Java中一种特殊的类,用于定义一组固定的常量,枚举类型提供了更好的类型安全性和可读性,适用于需要定义一组有限且固定的值的场景,本文给大家介绍Jav... 目录一、什么是枚举?二、枚举的基本使用方法定义枚举三、实际使用场景代替常量状态机四、更多用法1.实现接

springboot项目中使用JOSN解析库的方法

《springboot项目中使用JOSN解析库的方法》JSON,全程是JavaScriptObjectNotation,是一种轻量级的数据交换格式,本文给大家介绍springboot项目中使用JOSN... 目录一、jsON解析简介二、Spring Boot项目中使用JSON解析1、pom.XML文件引入依

Java中的record使用详解

《Java中的record使用详解》record是Java14引入的一种新语法(在Java16中成为正式功能),用于定义不可变的数据类,这篇文章给大家介绍Java中的record相关知识,感兴趣的朋友... 目录1. 什么是 record?2. 基本语法3. record 的核心特性4. 使用场景5. 自定

MyBatis编写嵌套子查询的动态SQL实践详解

《MyBatis编写嵌套子查询的动态SQL实践详解》在Java生态中,MyBatis作为一款优秀的ORM框架,广泛应用于数据库操作,本文将深入探讨如何在MyBatis中编写嵌套子查询的动态SQL,并结... 目录一、Myhttp://www.chinasem.cnBATis动态SQL的核心优势1. 灵活性与可