一个VO(Visual Odometry)的简单实现
2024-05-15 04:02:32
简单介绍
- 输入：指定数量的RGB+Depth深度图，相关参数配置文件.txt
- 输出：实时显示的三维模型，和最终的.pcd点云图
主函数
/*************************************************************************> File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp> Author: xiang gao> Mail: gaoxiang12@mails.tsinghua.edu.cn> Created Time: 2015年08月01日 星期六 15时35分42秒> Update: add comments by john 2020-10-16************************************************************************/#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;#include "slamBase.h"// 给定index，读取一帧数据
FRAME readFrame( int index, ParameterReader& pd );
// 度量运动的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );int main( int argc, char** argv )
{ParameterReader pd;int startIndex = atoi( pd.getData( "start_index" ).c_str() );int endIndex = atoi( pd.getData( "end_index" ).c_str() );// initializecout<<"Initializing ..."<<endl;int currIndex = startIndex; // 当前索引为currIndex//1. 读取当前帧的rgb+depth图像FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据// 我们总是在比较currFrame和lastFramestring detector = pd.getData( "detector" );string descriptor = pd.getData( "descriptor" );//2. 获取相机内参： Tracking::mpCameraCAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();//计算出lastFrame的关键点和描述子，存在当前帧结构中computeKeyPointsAndDesp( lastFrame, detector, descriptor );//3. 根据rgb+depth+相机内参，获取最后一帧相机坐标系下的点云。 JOHNTODO 可以使用PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );//4. 定义点云视图viewerpcl::visualization::CloudViewer viewer("viewer");// 是否显示点云bool visualize = pd.getData("visualize_pointcloud")==string("yes");int min_inliers = atoi( pd.getData("min_inliers").c_str() );double max_norm = atof( pd.getData("max_norm").c_str() );//5.遍历每一帧for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ){cout<<"Reading files "<<currIndex<<endl;FRAME currFrame = readFrame( currIndex,pd ); // 读取currFramecomputeKeyPointsAndDesp( currFrame, detector, descriptor );// 比较currFrame 和 lastFrame// JOHNCOMMENT: 获取当前帧currFrame关于上一帧的外参,此处用到图片的 descriptor// 注意: 此处的相机外参 result 中的R,t 是当前帧相对于上一帧的旋转和平移// 相机内参可以用于: "像素点坐标"到"相机坐标系下的空间点坐标"的转换// 相机外参可以用于: "相机坐标系下的空间点坐标"到"世界坐标系下的对应空间点坐标"的转换//6. 获取相机关于最后一帧的运动RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );//7. 过滤掉质量差的帧if ( result.inliers < min_inliers ) //inliers不够，放弃该帧continue; // 计算运动范围是否太大double norm = normofTransform(result.rvec, result.tvec);cout<<"norm = "<<norm<<endl;if ( norm >= max_norm )continue;Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );cout<<"T="<<T.matrix()<<endl;//8. 添加当前帧点云到历史点云中// JOHNCOMMENT: 将当前帧的点云加入到上一帧(或者说之前的历史)点云中，// 更具体地说: 将历史点云使用当前帧的位姿进行变换, 和当前点云对齐,然后加到当前点云上,// 使点云图在pcl的view中能一直以当前帧(或者说当前的相机位姿)的视角动起来// 点云变换函数: pcl::transformPointCloud( *original, *output, T.matrix());cloud = joinPointCloud( cloud, currFrame, T, camera );//9. 决定是否实时显示if ( visualize == true )viewer.showCloud( cloud );lastFrame = currFrame;}//10. 保存点云文件pcl::io::savePCDFile( "data/result.pcd", *cloud ); //保存最终的点云结果 JOHNTODOreturn 0;
}
//从配置文件pd中读取rgb+depth作为帧的属性，返回Frame
FRAME readFrame( int index, ParameterReader& pd )
{FRAME f;string rgbDir = pd.getData("rgb_dir");string depthDir = pd.getData("depth_dir");string rgbExt = pd.getData("rgb_extension");string depthExt = pd.getData("depth_extension");stringstream ss;ss<<rgbDir<<index<<rgbExt;string filename;ss>>filename;f.rgb = cv::imread( filename );ss.clear();filename.clear();ss<<depthDir<<index<<depthExt;ss>>filename;f.depth = cv::imread( filename, -1 );return f;
}double normofTransform( cv::Mat rvec, cv::Mat tvec )
{return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
}
效果
完整代码： https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V
数据位置：http://yun.baidu.com/s/1i33uvw5
参考：一起做RGB-D SLAM (5)
一个VO(Visual Odometry)的简单实现相关推荐
- 视觉里程计Visual Odometry(VO)
视觉里程计(Visual Odometry,简称VO)是SLAM技术中非常关键的部分,主要侧重于计算机视觉算法. 视觉里程计这个术语借鉴了汽车的车轮里程计的概念,还是挺贴切的. 我们知道汽车的车轮里程 ...
- Visual Odometry Revisited: What Should Be Learnt?（2020.2）
代码:https://github.com/Huangying-Zhan/DF-VO 文章:https://arxiv.org/abs/1909.09803 摘要 本文利用几何关系和深度学习提出单目视 ...
- 每天一篇论文 365/365 Visual Odometry Revisited: What Should Be Learnt?
Memory-Efficient Implementation of DenseNets Visual Odometry Revisited: What Should Be Learnt? Code ...
- 论文阅读：PVO: Panoptic Visual Odometry
全景视觉里程计.同时做全景分割和视觉里程计 连接:PVO: Panoptic Visual Odometry 0.Abstract 我们提出了一种新的全景视觉里程计框架PVO,以实现对场景运动.几何和 ...
- svo： semi-direct visual odometry 半直接视觉里程计 fast角点匹配 光流匹配 单应变换求位姿 直接法求解位姿 高斯均匀分布混合深度滤波
svo: semi-direct visual odometry 半直接视觉里程计 本博文github地址 svo代码注释 SVO代码分析 较细致 svo: semi-direct visual od ...
- 什么是视觉里程计（Visual Odometry）？
1.概念:什么是里程计? 在里程计问题中,我们希望测量一个运动物体的轨迹.这可以通过许多不同的手段来实现.例如,我们在汽车轮胎上安装计数码盘,就可以得到轮胎转动的距离,从而得到汽车的估计.或者,也可以 ...
- SVO（SVO: fast semi-direct monocular visual odometry）
SVO(SVO: fast semi-direct monocular visual odometry)翻译 文章目录 SVO(SVO: fast semi-direct monocular visu ...
- 学习笔记之——Semi-direct Visual Odometry (SVO)
本博文为本人学习SVO的学习笔记,本博文大部分内容来源于网络各种参考资料,本博文仅为本人学习记录用. 目录 原理 Tracking(位姿估计线程) 第一步:图像对齐 第二步:特征对齐 第三步:位姿结构 ...
- Paper阅读：VOLDOR: Visual Odometry from Log-logistic Dense Optical flow Residuals
VOLDOR:来自对数逻辑密集光流残差的视觉里程表 引用: Min, Z., Yang, Y., & Dunn, E. (2020). VOLDOR: Visual Odometry From ...
- 论文阅读——《Online Photometric Calibration of Auto Exposure Video for Realtime Visual Odometry and SLAM》
论文阅读--<Online Photometric Calibration of Auto Exposure Video for Realtime Visual Odometry and SLA ...
最新文章
- 按计算机的地位划分计算机网络可分为,2010年全国自考计算机网络基本原理模拟试卷(三)及答案...
- 5、删除被其它表关联的主表
- 吴恩达《机器学习》课程总结（19）总结
- 忘记token怎么加入k8s集群
- 19-6/24作业： 将一个double类型的小数，按照四舍五入保留两位小数
- Linux du 命令
- Atitti.软件的一些理论补充 Atitti.软件的原理原则定律法则补充 目录 1.1. 分布式领域CAP理论， 1 1.2. 关系数据库的ACID模型拥有 高一致性 + 可用性 很难进行分区：
- 今日直播预告 | 漏洞审计介绍
- 天涯论坛_全球华人网上家园_天涯社区
- 数据驱动的互联网营销和运营专用名词速览
- wince 触摸屏 学习
- Redis的持久化操作---RDBAOF
- c#精华网站收集（转摘自梅川酷子的博客）
- iOS常用方法——WKWebView缓存清理的实现
- Writing a code for speed tracking control of maglev trains in MATLAB using reinforcement learning
- Java 扫描识别条形码图片
- 藏宝阁上架服务器信息错误,苦行虚空，难道在处罚中也能上架藏宝阁吗？
- 手机的九宫格图案解锁总共能绘出多少种图案？LeetCode 351. Android Unlock Patterns
- Redis之性能指标、监控方式
- 风华秋实再度递表港交所：非控股权益亏损扩大，涉三七互娱等