对极几何与三角化求3D空间坐标

一，使用对极几何约束求R,T

第一步：特征匹配。提取出有效的匹配点

void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches) {//-- 初始化Mat descriptors_1, descriptors_2;// used in OpenCV3Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// use this if you are in OpenCV2// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//-- 第一步:检测 Oriented FAST 角点位置detector->detect(img_1, keypoints_1);detector->detect(img_2, keypoints_2);//-- 第二步:根据角点位置计算 BRIEF 描述子descriptor->compute(img_1, keypoints_1, descriptors_1);descriptor->compute(img_2, keypoints_2, descriptors_2);//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离vector<DMatch> match;// BFMatcher matcher ( NORM_HAMMING );matcher->match(descriptors_1, descriptors_2, match);//-- 第四步:匹配点对筛选double min_dist = 10000, max_dist = 0;//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for (int i = 0; i < descriptors_1.rows; i++) {double dist = match[i].distance;if (dist < min_dist) min_dist = dist;if (dist > max_dist) max_dist = dist;}printf("-- Max dist : %f \n", max_dist);printf("-- Min dist : %f \n", min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for (int i = 0; i < descriptors_1.rows; i++) {if (match[i].distance <= max(2 * min_dist, 30.0)) {matches.push_back(match[i]);}}
}

二、使用本质矩阵求解R，T

第二步：根据匹配点对，依据对极几何约束原理，求相机运动的R,t

void pose_estimation_2d2d(const std::vector<KeyPoint> &keypoints_1,const std::vector<KeyPoint> &keypoints_2,const std::vector<DMatch> &matches,Mat &R, Mat &t) {// 相机内参,TUM Freiburg2Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//-- 把匹配点转换为vector<Point2f>的形式vector<Point2f> points1;vector<Point2f> points2;for (int i = 0; i < (int) matches.size(); i++) {points1.push_back(keypoints_1[matches[i].queryIdx].pt);points2.push_back(keypoints_2[matches[i].trainIdx].pt);}//-- 计算本质矩阵Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值int focal_length = 521;            //相机焦距, TUM dataset标定值Mat essential_matrix;essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);//-- 从本质矩阵中恢复旋转和平移信息.recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
}

三、由R，T三角化空间坐标

第三步：根据针孔相机模型的公式，由 R,t估计特征点的空间坐标

//三角化,根据匹配点和求解到的三维点。存储在points中
void triangulation(const vector<KeyPoint> &keypoint_1,const vector<KeyPoint> &keypoint_2,const std::vector<DMatch> &matches,const Mat &R, const Mat &t,vector<Point3d> &points) {Mat T1 = (Mat_<float>(3, 4) <<1, 0, 0, 0,0, 1, 0, 0,0, 0, 1, 0);//根据求解到的RT构造T2矩阵Mat T2 = (Mat_<float>(3, 4) <<R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));//相机内参Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);vector<Point2f> pts_1, pts_2;for (DMatch m:matches) {// 将像素坐标转换至相机坐标pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));}Mat pts_4d;cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);// 转换成非齐次坐标for (int i = 0; i < pts_4d.cols; i++) {Mat x = pts_4d.col(i);x /= x.at<float>(3, 0); // 归一化Point3d p(x.at<float>(0, 0),x.at<float>(1, 0),x.at<float>(2, 0));points.push_back(p);}
}

其中 triangulatePoints()的具体用法为

triangulatePoints(T1, T2, left, right, points_final) ;Mat T1 = (Mat_<float>(3, 4) <<1, 0, 0, 0,0, 1, 0, 0,0, 0, 1, 0);
Mat T2 = (Mat_<float>(3, 4) <<R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), T.at<double>(0, 0),R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), T.at<double>(1, 0),R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), T.at<double>(2, 0));`
triangulatePoints(T1, T2, left, right, points_final) ;其中T2为3x4的[R|T]矩阵，left、right为相机坐标系下的归一化坐标，
因此不能直接使用提取到的像素坐标。应首先将像素坐标通过相机内参转化到相机坐标系下。

所以通过函数pixel2cam可将像素坐标转换到归一化相机坐标系下
归一化坐标：X=(u-u0)/fx

//像素坐标到归一化平面相机坐标的转换
Point2f pixel2cam(const Point2f& p, const Mat& K)
{return Point2f((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

四、代码demo

总的代码为：

#include <iostream>
#include <opencv2/opencv.hpp>
// #include "extra.h" // used in opencv2
using namespace std;
using namespace cv;void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches);void pose_estimation_2d2d(const std::vector<KeyPoint> &keypoints_1,const std::vector<KeyPoint> &keypoints_2,const std::vector<DMatch> &matches,Mat &R, Mat &t);void triangulation(const vector<KeyPoint> &keypoint_1,const vector<KeyPoint> &keypoint_2,const std::vector<DMatch> &matches,const Mat &R, const Mat &t,vector<Point3d> &points
);/// 作图用
inline cv::Scalar get_color(float depth) {float up_th = 50, low_th = 10, th_range = up_th - low_th;if (depth > up_th) depth = up_th;if (depth < low_th) depth = low_th;return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);int main(int argc, char **argv) {if (argc != 3) {cout << "usage: triangulation img1 img2" << endl;return 1;}//-- 读取图像Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);vector<KeyPoint> keypoints_1, keypoints_2;vector<DMatch> matches;find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);cout << "一共找到了" << matches.size() << "组匹配点" << endl;//-- 估计两张图像间运动Mat R, t;pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);//-- 三角化vector<Point3d> points;//tr是三维点triangulation(keypoints_1, keypoints_2, matches, R, t, tr);//-- 验证三角化点与特征点的重投影关系Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);Mat img1_plot = img_1.clone();Mat img2_plot = img_2.clone();for (int i = 0; i < matches.size(); i++) {// 第一个图float depth1 = points[i].z;cout << "depth: " << depth1 << endl;Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);// 第二个图Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;float depth2 = pt2_trans.at<double>(2, 0);cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);}cv::imshow("img 1", img1_plot);cv::imshow("img 2", img2_plot);cv::waitKey();return 0;
}void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches) {//-- 初始化Mat descriptors_1, descriptors_2;// used in OpenCV3Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> descriptor = ORB::create();// use this if you are in OpenCV2// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//-- 第一步:检测 Oriented FAST 角点位置detector->detect(img_1, keypoints_1);detector->detect(img_2, keypoints_2);//-- 第二步:根据角点位置计算 BRIEF 描述子descriptor->compute(img_1, keypoints_1, descriptors_1);descriptor->compute(img_2, keypoints_2, descriptors_2);//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离vector<DMatch> match;// BFMatcher matcher ( NORM_HAMMING );matcher->match(descriptors_1, descriptors_2, match);//-- 第四步:匹配点对筛选double min_dist = 10000, max_dist = 0;//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离for (int i = 0; i < descriptors_1.rows; i++) {double dist = match[i].distance;if (dist < min_dist) min_dist = dist;if (dist > max_dist) max_dist = dist;}printf("-- Max dist : %f \n", max_dist);printf("-- Min dist : %f \n", min_dist);//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.for (int i = 0; i < descriptors_1.rows; i++) {if (match[i].distance <= max(2 * min_dist, 30.0)) {matches.push_back(match[i]);}}
}void pose_estimation_2d2d(const std::vector<KeyPoint> &keypoints_1,const std::vector<KeyPoint> &keypoints_2,const std::vector<DMatch> &matches,Mat &R, Mat &t) {// 相机内参,TUM Freiburg2Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//-- 把匹配点转换为vector<Point2f>的形式vector<Point2f> points1;vector<Point2f> points2;for (int i = 0; i < (int) matches.size(); i++) {points1.push_back(keypoints_1[matches[i].queryIdx].pt);points2.push_back(keypoints_2[matches[i].trainIdx].pt);}//-- 计算本质矩阵Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值int focal_length = 521;            //相机焦距, TUM dataset标定值Mat essential_matrix;essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);//-- 从本质矩阵中恢复旋转和平移信息.recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
}//三角化,根据匹配点和求解到的三维点。存储在points中
void triangulation(const vector<KeyPoint> &keypoint_1,const vector<KeyPoint> &keypoint_2,const std::vector<DMatch> &matches,const Mat &R, const Mat &t,vector<Point3d> &points) {Mat T1 = (Mat_<float>(3, 4) <<1, 0, 0, 0,0, 1, 0, 0,0, 0, 1, 0);//根据求解到的RT构造T2矩阵Mat T2 = (Mat_<float>(3, 4) <<R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));//相机内参Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);vector<Point2f> pts_1, pts_2;for (DMatch m:matches) {// 将像素坐标转换至相机坐标pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));}Mat pts_4d;cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);// 转换成非齐次坐标for (int i = 0; i < pts_4d.cols; i++) {Mat x = pts_4d.col(i);x /= x.at<float>(3, 0); // 归一化Point3d p(x.at<float>(0, 0),x.at<float>(1, 0),x.at<float>(2, 0));points.push_back(p);}
}Point2f pixel2cam(const Point2d &p, const Mat &K) {return Point2f((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

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

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

相关文章

Python超入门（1）__迅速上手操作掌握Python

Python超入门（1）__迅速上手操作掌握Python

# 1.第一个代码&#xff1a;输出语句 # 1.第一个代码&#xff1a;输出语句 print("My dogs name is Huppy!") print(o----) print( ||| ) print("*" * 10) """ 输出结果&#xff1a; My dogs name is Huppy! o----||| ********** "&…
阅读更多...
RK3588算法盒子maskrom模式下系统烧录

RK3588算法盒子maskrom模式下系统烧录

先在firefly官网下载bulidroot文件&#xff0c;然后进行烧录相关文件。 点击upgrade&#xff0c;进行烧录升级&#xff1b; 等待烧录完成后&#xff0c; 重新开机进入loader模式下&#xff0c; 进行烧录Untunt20.04系统的操作就行。
阅读更多...
(16)线程的实例认识:Await,Async,ConfigureAwait

(16)线程的实例认识:Await,Async,ConfigureAwait

继续(15)的例子 一、ConfigureAwait()的作用 private async void BtnAsync_Click(object sender, EventArgs e)//异步{Stopwatch sw Stopwatch.StartNew();TxtInfo.Clear();AppendLine("异步检索开始...");AppendLine($"当前线程Id:{Environment.CurrentManage…
阅读更多...
springboot使用切面记录接口访问日志

springboot使用切面记录接口访问日志

前言 当我们开发和维护一个复杂的应用程序时&#xff0c;了解应用程序的运行情况变得至关重要。特别是在生产环境中&#xff0c;我们需要追踪应用程序的各个方面&#xff0c;以确保它正常运行并能够及时发现潜在的问题。其中之一关键的方面是记录应用程序的接口访问日志。 Sp…
阅读更多...
函数相关概念

函数相关概念

4.函数 1.函数的概念 1.什么是函数? 把特点的代码片段,抽取成为独立运行的实体 2.使用函数的好处1.重复使用,提供效率2.提高代码的可读性3.有利用程序的维护 3.函数的分类1.内置函数(系统函数)已经提高的alert(); prompt();confirm();print()document.write(),console.log()…
阅读更多...
http和https区别，第三方证书如何保证服务器可信

http和https区别，第三方证书如何保证服务器可信

目录 HTTP和HTTPS有以下区别&#xff1a; 第三方证书如何保证服务器的可信性 需要注意哪些方面 可能遇到什么问题 随着互联网技术的不断发展&#xff0c;数据传输的安全性越来越受到人们的关注。HTTP和HTTPS是两种常用的网络协议&#xff0c;它们之间的主要区别在于数据传输…
阅读更多...
【图文并茂】c++介绍之队列

【图文并茂】c++介绍之队列

1.1队列的定义 队列&#xff08;queue&#xff09;简称队&#xff0c;它也是一种操作受限的线性表&#xff0c;其限制为仅允许在表的一端进行插入操作&#xff0c;而在表的另一端进行删除操作 一些基础概念&#xff1a; 队尾&#xff08;rear&#xff09; &#xff1a;进行插…
阅读更多...
CloudQuery X PolarDB：让数据库管理更简单

CloudQuery X PolarDB：让数据库管理更简单

前言&#xff1a;8 月 15 日&#xff0c;CloudQuery 数据操作管控平台与阿里云 PolarDB 数据库管理软件&#xff0c;完成产品集成认证测试。也在以下功能上完善了用户使用 PolarDB 的体验&#xff0c;使数据库的管理更加安全高效。 支持在 CloudQuery 中创建连接&#xff0c;便…
阅读更多...
数据接口工程对接BI可视化大屏(一)

数据接口工程对接BI可视化大屏(一)

文章目录 第1章 案例概述1.1 案例目标1.2 BI最终效果1.2.1 PC端显示效果1.2.2 移动端显示效果 后记 第1章 案例概述 1.1 案例目标 此项目以常见的手机零售BI场景为例&#xff0c;介绍如何编写数据接口工程对接BI可视化大屏。 如何从当前常见的主流大数据场景中为后台程序推送…
阅读更多...
数据结构入门-13-图

数据结构入门-13-图

文章目录 一、图的概述1.1 图论的作用1.2 图的分类1.2.1 无向图1.2.2 有向图1.2.3 无权图1.2.4 有劝图 1.3 图的基本概念 二、树的基本表示2.1 邻接矩阵2.1.1 邻接矩阵 表示图2.1.2 邻接矩阵的复杂度 2.2 邻接表2.2.1 邻接表的复杂度2.2.2 邻接表By哈希表 三、图的深度优先遍历…
阅读更多...
sentinel加密狗使用及规则配置

sentinel加密狗使用及规则配置

Sentinel加密狗是一种硬件加密设备&#xff0c;用于保护软件应用程序免受未经授权的访问和复制。它可以提供软件许可管理、访问控制和数据保护等功能。下面是Sentinel加密狗的使用及规则配置的相关介绍。 Sentinel加密狗的使用 插入加密狗&#xff1a;将Sentinel加密狗插入计算…
阅读更多...
C51智能小车（循迹、跟随、避障、测速、蓝牙、wifie、4g、语音识别）总结

C51智能小车（循迹、跟随、避障、测速、蓝牙、wifie、4g、语音识别）总结

目录 1.电机模块开发 1.1 让小车动起来 1.2 串口控制小车方向 1.3 如何进行小车PWM调速 1.4 PWM方式实现小车转向 2.循迹小车 2.1 循迹模块使用 2.2 循迹小车原理 2.3 循迹小车核心代码 3.跟随/避障小车 3.1 红外壁障模块分析​编辑 3.2 跟随小车的原理 3.3 跟随小…
阅读更多...
Leetcode.174 地下城游戏

Leetcode.174 地下城游戏

题目链接 Leetcode.174 地下城游戏 hard 题目描述 恶魔们抓住了公主并将她关在了地下城 d u n g e o n dungeon dungeon 的 右下角 。地下城是由 m x n 个房间组成的二维网格。我们英勇的骑士最初被安置在 左上角 的房间里&#xff0c;他必须穿过地下城并通过对抗恶魔来拯救公…
阅读更多...
【高阶产品策略】设计有效的AB测试

【高阶产品策略】设计有效的AB测试

文章目录 1、A/B测试概述2、A/B测试实施过程3、A/B测试中需要注意的地方4、从一个案例中看A/B测试 1、A/B测试概述 2、A/B测试实施过程 3、A/B测试中需要注意的地方 4、从一个案例中看A/B测试
阅读更多...
编写中间件以用于 Express 应用程序

编写中间件以用于 Express 应用程序

概述 中间件函数能够访问请求对象 (req)、响应对象 (res) 以及应用程序的请求/响应循环中的下一个中间件函数。下一个中间件函数通常由名为 next 的变量来表示。 中间件函数可以执行以下任务&#xff1a; 执行任何代码。对请求和响应对象进行更改。结束请求/响应循环。调用堆…
阅读更多...
pytorch学习——循环神经网络RNN讲解及其实现

pytorch学习——循环神经网络RNN讲解及其实现

参考书籍&#xff1a;8.6. 循环神经网络的简洁实现 — 动手学深度学习 2.0.0 documentation 参考视频&#xff1a;54 循环神经网络 RNN【动手学深度学习v2】_哔哩哔哩_bilibili 一.介绍 循环神经网络RNN&#xff08;Recurrent Neural Network &#xff09;是一类广泛应用于序列…
阅读更多...
stm32之30.DMA

stm32之30.DMA

DMA&#xff08;硬件加速方法&#xff09;一般用于帮运比较大的数据&#xff08;如&#xff1a;摄像头数据图像传输&#xff09;&#xff0c;寄存器-》DMA-》RAM 或者 RAM-》DMA-》寄存器提高CPU的工作效率 源码-- #include "myhead.h" #include "adc.h"#…
阅读更多...
javaee之黑马乐优商城2

javaee之黑马乐优商城2

简单分析一下商品分类表的结构 先来说一下分类表与品牌表之间的关系 再来说一下分类表和品牌表与商品表之间的关系 面我们要开始就要创建sql语句了嘛&#xff0c;这里我们分析一下字段 用到的数据库是heima->tb_category这个表 现在去数据库里面创建好这张表 下面我们再去编…
阅读更多...
有向图和无向图的表示方式(邻接矩阵，邻接表)

有向图和无向图的表示方式(邻接矩阵，邻接表)

目录 一.邻接矩阵 1.无向图​编辑 2.有向图 补充&#xff1a;网&#xff08;有权图&#xff09;的邻接矩阵表示法 二.邻接表 1.无向图 2.有向图 三.邻接矩阵与邻接表的关系 一.邻接矩阵 1.无向图 &#xff08;1&#xff09;对角线上是每一个顶点与自身之间的关系&…
阅读更多...
线性空间和线性变化

线性空间和线性变化

目录 考点一、线性空间的基与维数 1、线性空间 2、基底 3、子空间&#xff08;线性子空间&#xff09; ​编辑4、生成子空间 &#xff08;1&#xff09;、v1 n v2 &#xff08;2&#xff09;、v1 v2 5、求和子空间的方法 6、维数定理 7、例题 &#xff08;1&#xf…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023