手撕 视觉slam14讲 ch7 / pose_estimation_3d2d.cpp (1)

首先理清我们需要实现什么功能,怎么实现,提供一份整体逻辑:包括主函数和功能函数

主函数逻辑:

 1. 读图,两张rgb(cv::imread)

 2. 找到两张rgb图中的特征点匹配对

       2.1定义所需要的参数:keypoints1, keypoints2,matches

       2.2 提取每张图像的检测 Oriented FAST 角点位置并匹配筛选(调用功能函数1)

 3. 建立3d点(像素坐标到相机坐标)

        3.1读出深度图(cv::imread)

        3.2取得每个匹配点对的深度

                3.2.1 得到第y行,第x个像素的深度值

                   (ushort d = d1.ptr<unsigned short> (row)[column])

                3.2.2 去除没有深度的点

                3.2.3 转到相机坐标系(调用功能函数2)

4. 调用epnp求解(input:3d点,2d点对,内参,是否去畸变,求解方式)

        4.1求解(cv::solvePnP)

         4.2 求解结果为向量,需要转成矩阵(cv::Rodrigues)

int main( int agrc, char** agrv) {
//  1. 读图(两张rgb)Mat image1 = imread(agrv[1] , CV_LOAD_IMAGE_COLOR );Mat image2 = imread(agrv[2] , CV_LOAD_IMAGE_COLOR );assert(image1.data && image2.data && "Can not load images!");//  2. 找到两张rgb图中的特征点匹配对// 2.1定义keypoints1, keypoints2,matchesstd::vector<KeyPoint>keypoints1,keypoints2;std::vector<DMatch>matches;// 2.2 提取每张图像的检测 Oriented FAST 角点位置并匹配筛选Featurematcher(image1,image2, keypoints1,keypoints2,matches);//  3. 建立3d点(像素坐标到相机坐标)Mat K  = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//内参vector<Point3f> pts_3d;vector<Point2f> pts_2d;//3.1读出深度图Mat d1 =imread(agrv[3],CV_LOAD_IMAGE_UNCHANGED);//3.2取得每个匹配点对的深度(ushort d = d1.ptr<unsigned short> (row)[column];就是指向d1的第row行的第column个数据。数据类型为无符号的短整型 )for (DMatch m: matches){//3.2.1 得到第y行,第x个位置的像素的深度值ushort d = d1.ptr<unsigned short>(int (keypoints1[m.queryIdx].pt.y)) [int(keypoints1[m.queryIdx].pt.x)];// 3.2.2 去除没有深度的点if(d==0){continue;}float dd=d/5000.0 ;//3.2.3 转到相机坐标系Point2d p1 = pixtocam(keypoints1[m.queryIdx].pt , K);pts_3d.push_back(Point3f(p1.x*dd,p1.y*dd,dd));pts_2d.push_back(keypoints2[m.trainIdx].pt);}cout << "3d-2d pairs: " << pts_3d.size() << endl;//  4. 调用epnp求解(input:3d点,2d点对,内参,false,求解方式)// solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );Mat r,t;// 4.1求解solvePnP(pts_3d,pts_2d,K,Mat(), r,t,false,SOLVEPNP_EPNP);// 4.2 求解结果为向量,需要转成矩阵Mat R;cv::Rodrigues(r,R);cout<<"R="<<R<<endl;cout<<"T="<<t<<endl;// 5.可视化匹配Mat img_goodmatch;drawMatches(image1, keypoints1, image2, keypoints2, matches, img_goodmatch);imshow("good matches", img_goodmatch);waitKey(0);return 0;
}

功能函数1:  Featurematcher

实现过程在前几篇中已经详细说明:视觉slam14讲 逐行解析代码 ch7 / orb_cv.cpp

2.2.1初始化存储特征点数据的变量

2.2.2 提取每张图像的检测 Oriented FAST 角点位置

2.2.3 计算图像角点的BRIEF描述子

2.2.4 根据刚刚计算好的BRIEF描述子,对两张图的角点进行匹配

2.2.5 匹配点对筛选计算最小距离和最大距离

2.2.6 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.

void Featurematcher( const Mat &image1, const Mat &image2, std::vector<KeyPoint>&keypoints1, std::vector<KeyPoint> &keypoints2,  std::vector<DMatch> &matches){// 2.2.1初始化存储特征点数据的变量Mat descr1, descr2;Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");// 2.2.2 提取每张图像的检测 Oriented FAST 角点位置detector->detect(image1, keypoints1);detector->detect(image2, keypoints2);// 2.2.3 计算图像角点的BRIEF描述子descriptor->compute(image1, keypoints1, descr1);descriptor->compute(image2, keypoints2, descr2);// 2.2.4 根据刚刚计算好的BRIEF描述子,对两张图的角点进行匹配std::vector<DMatch> match;matcher->match(descr1, descr2, match);Mat img_match;drawMatches(image1, keypoints1, image2, keypoints2, match, img_match);imshow("all matches", img_match);waitKey(0);// 2.2.5 匹配点对筛选计算最小距离和最大距离double min_dis = 10000, max_dis = 0;// 2.2.5.1找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for (int i = 0; i < descr1.rows; i++){double dist = match[i].distance;if (dist < min_dis)min_dis = dist;if (dist > max_dis)max_dis = dist;}cout<<"max_dis="<<max_dis<<endl;cout<<"min_dis="<<min_dis<<endl;//2.2.6 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for (int i = 0; i < descr1.rows; i++){if (match[i].distance<= max(2*min_dis,30.0)){matches.push_back(match[i]);}       }cout<<"matches.size="<<matches.size()<<endl;
}

功能函数2:

将输入的像素坐标(x ,y)转化到归一化相机坐标系下得到(X,Y)

我们知道:相机的投影模型为:u=KP, 即

\begin{bmatrix} x \\ y \\ 1 \end{bmatrix}=\begin{bmatrix} f_{x} &0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}

所以 X=(x-c_x)/f_x     ,    Y=(y-c_y)/f_y

Point2d pixtocam(const  Point2d &p ,  const Mat  &K){return Point2d(// X=(u-cx)/fx(p.x - K.at<double>(0,2)) / K.at<double>(0,0) ,// Y=(v-cy)/fy(p.y-K.at<double>(1,2)) / K.at<double>(1,1));
}

最后匹配效果及位姿结果:

allmatch:

goodmatch:

位姿输出:R,T:

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/65632.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

VMware 安装 Centos7 超详细过程

CentOS系统&#xff0c;安装教程可参考以下&#xff1a; 哪些模型需要在Linux下运行&#xff0c;需提前预装Linux系统呢&#xff0c;评论区讨论吧 比如Noah-MP 5.0模型 1.软硬件准备 软件&#xff1a;推荐使用 VMware&#xff0c;我用的是 VMware 12 镜像&#xff1a;CentO…

【python爬虫】10.指挥浏览器自动工作(selenium)

文章目录 前言selenium是什么怎么用设置浏览器引擎获取数据解析与提取数据自动操作浏览器 实操运用确认目标分析过程代码实现 本关总结 前言 上一关&#xff0c;我们认识了cookies和session。 分别学习了它们的用法&#xff0c;以及区别。 还做了一个项目&#xff1a;带着小…

MTK6761/MT6761安卓核心板4G安卓智能模块详细参数性能介绍

MTK6761 安卓核心板采用12nm制程四核Cortex-A53、最高主频2.0GHZ 处理器&#xff0c;板载内存为 1GB8GB(2GB16GB、3GB32GB、4GB64GB)&#xff0c;搭载Android 9.0操作系统。 MTK6761&#xff08;曦力 A22&#xff09;安卓核心板基本概述 MTK6761安卓核心板 是一款高性能低功耗…

【算法奥义】最大矩形问题

首先建立一个二维数组&#xff0c;这个二维数组&#xff0c;计算出矩阵的每个元素的左边连续 1 的数量&#xff0c;使用二维数组 left记录&#xff0c;其中left[i][j] 为矩阵第 i 行第 j 列元素的左边连续 1 的数量。 也就是从这个元素开始&#xff0c;从右往左边数有多少个连…

Socks5代理 vs. Socks4代理:特点和区别解析

在网络通信中&#xff0c;使用代理服务器可以提供更安全、匿名的连接。其中&#xff0c;Socks5和Socks4是两种常见的代理协议。本文将深入探讨它们之间的特点和区别&#xff0c;帮助您选择适合自己需求的代理类型。 1.特点概述 -Socks5&#xff08;Socket Secure 5&#xff0…

SpringCloudGateway集成SpringDoc

SpringCloudGateway集成SpringDoc 最近在搞Spring版本升级&#xff0c;按客户要求升级Spring版本&#xff0c;原来用着SpringBoot 2.2.X版本&#xff0c;只需要升级SpringBoot 2.X最新版本也就可以满足客户Spring版本安全要求&#xff0c;可是好像最新的SpringBoot 2.X貌似也不…

【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER

文章目录 前言SPEED_DECIDER功能简介SPEED_DECIDER相关配置SPEED_DECIDER流程MakeObjectDecisionGetSTLocationCheck类函数CheckKeepClearCrossableCheckStopForPedestrianCheckIsFollowCheckKeepClearBlocked Create类函数 前言 在Apollo星火计划学习笔记——Apollo路径规划算…

Java-API简析_java.net.Inet4Address类(基于 Latest JDK)(浅析源码)

【版权声明】未经博主同意&#xff0c;谢绝转载&#xff01;&#xff08;请尊重原创&#xff0c;博主保留追究权&#xff09; https://blog.csdn.net/m0_69908381/article/details/132643590 出自【进步*于辰的博客】 因为我发现目前&#xff0c;我对Java-API的学习意识比较薄弱…

Ansible学习笔记8

group模块&#xff1a; 创建一个group组&#xff1a; [rootlocalhost ~]# ansible group1 -m group -a "nameaaa gid5000" 192.168.17.105 | CHANGED > {"ansible_facts": {"discovered_interpreter_python": "/usr/bin/python"}…

Linux线程控制

目录 一、线程创建 1.1 pthread_create 1.2 线程传入启动函数参数方式 二、线程退出(pthread_exit函数 pthread_cancel函数) 三、线程等待 3.1 为什么要线程等待&#xff1f; 3.2 pthread_join函数 四、线程分离 4.1 pthread_detach() 和 pthread_self() 五、pthread库…

Gteam2.0免授权毛玻璃拟态UI带后台版本修复版

程序使用PHP7版本运行 后台信息/Admin 账号admin 密码123456 后台功能 多管理员、系统日志等等功能

机器学习---决策树的划分依据(熵、信息增益、信息增益率、基尼值和基尼指数)

1. 熵 物理学上&#xff0c;熵 Entropy 是“混乱”程度的量度。 系统越有序&#xff0c;熵值越低&#xff1b;系统越混乱或者分散&#xff0c;熵值越⾼。 1948年⾹农提出了信息熵&#xff08;Entropy&#xff09;的概念。 从信息的完整性上进⾏的描述&#xff1a;当系统的有序…

Ansible 自动化运维工具的使用

目录 一、Ansible简介 二、Ansible 的安装和使用 1.下载 2.使用 三、Ansible命令和模块 1.命令格式 2.命令行模块 &#xff08;1&#xff09;command 模块 &#xff08;2&#xff09;shell 模块 &#xff08;3&#xff09;cron 模块 &#xff08;4&#xff09;user 模…

Flink 如何处理反压?

分析&回答 什么是反压&#xff08;backpressure&#xff09; 反压通常是从某个节点传导至数据源并降低数据源&#xff08;比如 Kafka consumer&#xff09;的摄入速率。反压意味着数据管道中某个节点成为瓶颈&#xff0c;处理速率跟不上上游发送数据的速率&#xff0c;而…

【IIS搭建网站】本地电脑做服务器搭建web站点并公网访问「内网穿透」

1.前言 在网上各种教程和介绍中&#xff0c;搭建网页都会借助各种软件的帮助&#xff0c;比如网页运行的Apache和Nginx、数据库软件MySQL和MSSQL之类&#xff0c;为方便用户使用&#xff0c;还出现了XAMPP、PHPStudy、宝塔面板等等一系列集成服务&#xff0c;都是为了方便我们…

linux并发服务器 —— 多进程并发 - 进程间的通信及实践(五)

进程间的通信 进程是一个独立的资源分配单元&#xff0c;不能在一个进程中直接访问另一个进程的资源&#xff1b; 进程间通信&#xff08;IPC&#xff09;的目的&#xff1a; 1. 数据传输 - A进程发送数据给B进程 2. 通知事件 - eg. 进程终止通知父进程 3. 资源共享 - 多个…

go语言 go mod生成

1. go hello world 创建文件夹gotest&#xff0c;在其中创建test1.go文件&#xff0c;并写入 package mainimport ("fmt" )func main() {fmt.Println("hello world") } 运行命令 go run test1.go 可以看到输出hello world 2. cli 命令行的使用 代码如下…

机器人中的数值优化(六)—— 线搜索最速下降法

本系列文章主要是我在学习《数值优化》过程中的一些笔记和相关思考&#xff0c;主要的学习资料是深蓝学院的课程《机器人中的数值优化》和高立编著的《数值最优化方法》等&#xff0c;本系列文章篇数较多&#xff0c;不定期更新&#xff0c;上半部分介绍无约束优化&#xff0c;…

重新理解百度智能云:写在大模型开放后的24小时

在这些回答背后共同折射出的一个现实是——大模型不再是一个单选题&#xff0c;而更是一个综合题。在这个新的时代帆船上&#xff0c;产品、服务、安全、开放等全部都需要成为必需品&#xff0c;甚至是从企业的落地层面来看&#xff0c;这些更是刚需品。 作者| 皮爷 出品|产…

c语言每日一练(13)

前言&#xff1a;每日一练系列&#xff0c;每一期都包含5道选择题&#xff0c;2道编程题&#xff0c;博主会尽可能详细地进行讲解&#xff0c;令初学者也能听的清晰。每日一练系列会持续更新&#xff0c;上学期间将看学业情况更新。 五道选择题&#xff1a; 1、程序运行的结果…