双目相机下目标三维坐标计算(四)
本文来自公众号:机器人视觉
完成双目相机标定以后,获得双目相机的参数矩阵
包括左右相机的内参数矩阵、左右相机的畸变系数矩阵、右相机相对于左相机的旋转矩阵与平移矩阵
已知左右相机图像中的对应点坐标,获取目标在双目视觉传感器下三维坐标的流程如下:
1、将双目相机标定参数整理如下:
//左相机内参数矩阵
float leftIntrinsic[3][3] = { 3061.6936, -0.8869, 641.3042,
0, 3058.8751, 508.9555,
0, 0, 1 };//左相机畸变系数
float leftDistortion[1][5] = { -0.0133, 0.6503, 0.0029, -0.0049, -16.8704 };
//左相机旋转矩阵
float leftRotation[3][3] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
//左相机平移向量
float leftTranslation[1][3] = { 0, 0, 0 };//右相机内参数矩阵
float rightIntrinsic[3][3] = { 3069.2482, -0.8951, 620.5357,
0, 3069.2450, 532.7122,
0, 0, 1 };
//右相机畸变系数
float rightDistortion[1][5] = { -0.0593, 3.4501, 0.0003, -8.5614, -58.3116 };
//右相机旋转矩阵
float rightRotation[3][3] = { 0.9989, 0.0131, -0.0439,
-0.0121, 0.9996, 0.0233,
0.0441, -0.0228, 0.9987};
//右相机平移向量
float rightTranslation[1][3] = {-73.8389, 2.6712, 3.3792};
2、二维像素坐标与相机坐标系下三维坐标转换
//************************************
// Description: 根据左右相机中成像坐标求解空间坐标
// Method: uv2xyz
// FullName: uv2xyz
// Parameter: Point2f uvLeft
// Parameter: Point2f uvRight
// Returns: cv::Point3f
//************************************
Point3f uv2xyz(Point2f uvLeft, Point2f uvRight)
{// [u1] |X| [u2] |X|//Z*|v1| = Ml*|Y| Z*|v2| = Mr*|Y|// [ 1] |Z| [ 1] |Z|// |1| |1|Mat mLeftRotation = Mat(3, 3, CV_32F, leftRotation);Mat mLeftTranslation = Mat(3, 1, CV_32F, leftTranslation);Mat mLeftRT = Mat(3, 4, CV_32F);//左相机M矩阵hconcat(mLeftRotation, mLeftTranslation, mLeftRT);Mat mLeftIntrinsic = Mat(3, 3, CV_32F, leftIntrinsic);Mat mLeftM = mLeftIntrinsic * mLeftRT;//cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;Mat mRightRotation = Mat(3, 3, CV_32F, rightRotation);Mat mRightTranslation = Mat(3, 1, CV_32F, rightTranslation);Mat mRightRT = Mat(3, 4, CV_32F);//右相机M矩阵hconcat(mRightRotation, mRightTranslation, mRightRT);Mat mRightIntrinsic = Mat(3, 3, CV_32F, rightIntrinsic);Mat mRightM = mRightIntrinsic * mRightRT;//cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;//最小二乘法A矩阵Mat A = Mat(4, 3, CV_32F);A.at<float>(0, 0) = uvLeft.x * mLeftM.at<float>(2, 0) - mLeftM.at<float>(0, 0);A.at<float>(0, 1) = uvLeft.x * mLeftM.at<float>(2, 1) - mLeftM.at<float>(0, 1);A.at<float>(0, 2) = uvLeft.x * mLeftM.at<float>(2, 2) - mLeftM.at<float>(0, 2);A.at<float>(1, 0) = uvLeft.y * mLeftM.at<float>(2, 0) - mLeftM.at<float>(1, 0);A.at<float>(1, 1) = uvLeft.y * mLeftM.at<float>(2, 1) - mLeftM.at<float>(1, 1);A.at<float>(1, 2) = uvLeft.y * mLeftM.at<float>(2, 2) - mLeftM.at<float>(1, 2);A.at<float>(2, 0) = uvRight.x * mRightM.at<float>(2, 0) - mRightM.at<float>(0, 0);A.at<float>(2, 1) = uvRight.x * mRightM.at<float>(2, 1) - mRightM.at<float>(0, 1);A.at<float>(2, 2) = uvRight.x * mRightM.at<float>(2, 2) - mRightM.at<float>(0, 2);A.at<float>(3, 0) = uvRight.y * mRightM.at<float>(2, 0) - mRightM.at<float>(1, 0);A.at<float>(3, 1) = uvRight.y * mRightM.at<float>(2, 1) - mRightM.at<float>(1, 1);A.at<float>(3, 2) = uvRight.y * mRightM.at<float>(2, 2) - mRightM.at<float>(1, 2);//最小二乘法B矩阵Mat B = Mat(4, 1, CV_32F);B.at<float>(0, 0) = mLeftM.at<float>(0, 3) - uvLeft.x * mLeftM.at<float>(2, 3);B.at<float>(1, 0) = mLeftM.at<float>(1, 3) - uvLeft.y * mLeftM.at<float>(2, 3);B.at<float>(2, 0) = mRightM.at<float>(0, 3) - uvRight.x * mRightM.at<float>(2, 3);B.at<float>(3, 0) = mRightM.at<float>(1, 3) - uvRight.y * mRightM.at<float>(2, 3);Mat XYZ = Mat(3, 1, CV_32F);//采用SVD最小二乘法求解XYZsolve(A, B, XYZ, DECOMP_SVD);//cout<<"空间坐标为 = "<<endl<<XYZ<<endl;//世界坐标系中坐标Point3f world;world.x = XYZ.at<float>(0, 0);world.y = XYZ.at<float>(1, 0);world.z = XYZ.at<float>(2, 0);return world;
}//************************************
// Description: 将世界坐标系中的点投影到左右相机成像坐标系中
// Method: xyz2uv
// FullName: xyz2uv
// Parameter: Point3f worldPoint
// Parameter: float intrinsic[3][3]
// Parameter: float translation[1][3]
// Parameter: float rotation[3][3]
// Returns: cv::Point2f
//************************************
Point2f xyz2uv(Point3f worldPoint, float intrinsic[3][3], float translation[1][3], float rotation[3][3])
{// [fx s x0] [Xc] [Xw] [u] 1 [Xc]//K = |0 fy y0| TEMP = [R T] |Yc| = TEMP*|Yw| | | = —*K *|Yc|// [ 0 0 1 ] [Zc] |Zw| [v] Zc [Zc]// [1 ]Point3f c;c.x = rotation[0][0] * worldPoint.x + rotation[0][1] * worldPoint.y + rotation[0][2] * worldPoint.z + translation[0][0] * 1;c.y = rotation[1][0] * worldPoint.x + rotation[1][1] * worldPoint.y + rotation[1][2] * worldPoint.z + translation[0][1] * 1;c.z = rotation[2][0] * worldPoint.x + rotation[2][1] * worldPoint.y + rotation[2][2] * worldPoint.z + translation[0][2] * 1;Point2f uv;uv.x = (intrinsic[0][0] * c.x + intrinsic[0][1] * c.y + intrinsic[0][2] * c.z) / c.z;uv.y = (intrinsic[1][0] * c.x + intrinsic[1][1] * c.y + intrinsic[1][2] * c.z) / c.z;return uv;
}
3、由像素坐标获取三维坐标
Point2f l = (638, 393);Point2f r = (85, 502);Point3f worldPoint;worldPoint = uv2xyz(l, r);cout << "空间坐标为:" << endl << uv2xyz(l, r) << endl;
更换点对测试
Point2f l = (857, 666);Point2f r = (303, 775);//Point2f l = (1014, 445);//Point2f r = (523, 387);Point3f worldPoint;worldPoint = uv2xyz(l, r);cout << "空间坐标为:" << endl << uv2xyz(l, r) << endl;system("pause");
更换点对测试:
Point2f l = (931, 449);Point2f r = (370, 555);Point3f worldPoint;worldPoint = uv2xyz(l, r);cout << "空间坐标为:" << endl << uv2xyz(l, r) << endl;system("pause");
线结构光传感器标定(相机标定+结构光标定)完整流程(一)
https://blog.csdn.net/qq_27353621/article/details/120787942
UR机器人手眼标定(二)
https://blog.csdn.net/qq_27353621/article/details/121603215
双目相机标定(三)
https://blog.csdn.net/qq_27353621/article/details/121031972
公众号:机器人视觉
双目相机下目标三维坐标计算(四)相关推荐
- 基于OpenCV的立体相机标定StereoCalibration与目标三维坐标定位
说明:以下涉及到的一些公式以及图片来自于Learning OpenCV. 做了快2个月的立体相机标定,遇到了一些问题,也有了一些体会,在这里记下来. 1.在做立体相机标定的时候,标定板的规范与否直接影 ...
- Realsense获取像素点在相机坐标系下的三维坐标
系统:Windows10 设备:RealSense D435i 提前搭建好Python环境,安装pyrealsense2,numpy,opencv. python-代码 # -*- coding: u ...
- 【双目相机同目标追踪】
多摄像头多目标定位追踪相关原理: 1. 多摄像头多目标追踪指标(Multi-Target Multi-Camera Tracking, MTMC Tracking) 2. 多相机系统视觉定位中的高效2 ...
- tof相机简介及三维坐标转化,plotly画3D点云
最近在做TOF相机相关的软件,近年来tof相机开始在手机,车载设备,VR等应用开始增多,产业也开始量化,是一个不错的3维相机的方向. 简单介绍一下tof相机吧:TOF是Time of flight的简 ...
- java求外接圆圆心_由三点三维坐标计算圆半径和圆心位置
[实例简介] 通过输入任意三点空间坐标自动判断三点是否在一条直线上,若不在则求三点外接圆的圆心位置和半径,计算精度可调节.解决了"知三点求圆心半径"程序对于某些点无法计算的缺陷. ...
- 三维坐标计算马氏距离(Python)
前言 搜索这个标题时看到好多都是用二维数据算马氏距离,找了半天看到几个三维的,但语言是matlab,所以就写个python的记录一下.本文适用于知道这是个距离,不想了解公式,copy直接能用的情况. ...
- 双目相机图像校正(五)
本文来自公众号:机器人视觉 1.读取左右相机图像,使用Opencv和MATLAB标定结果对图像进行校正 //对焊缝图像进行校准//load imageMat img_left = imread(&qu ...
- 三维重建(5)之三角测量计算双目相机坐标系下三维坐标
三角测量计算三维坐标 注:三角测量须在双目相机进行立体匹配之后进行计算,针对极线校正后图像 关于极线校正可以参考我的上篇博文:相机成像+单应性变换+相机标定+立体校正 1.双目相机成像原理详解 参考: ...
- 【OpenCV】双目相机测距及其深度恢复原理及其算法流程
1. 数学模型 2.整体流程 获取标定与图像数据==>stereoRectify==>initUndistortRectifyMap==>remap==>bg/sgbm恢复出视 ...
最新文章
- poj2728(最优比率生成树)
- javascript(arguments)
- laytpl : 一款非常轻量的JavaScript模板引擎
- java中ThreadLocalRandom的使用
- 湘乡江南计算机学校,湘乡职业中等专业学校2021年招生录取分数线
- 程序员去交友网站找女友,没想到找到了这个...
- Redis缓存异常的容错实现方法( .net)
- ACL2020 | 词向量性别偏见
- mysql改根用户_MySQL数据库中复位根用户密码的方法 | 很文博客
- java jdk 8 中文文档
- 【肌电信号】肌电信号处理系统含Matlab源码
- 查询中国天气网api需要用到的城市代码
- 解决退格键在MinGW的vim中不起作用的问题
- 两圆的外切线与内切线的切点算法
- 强网杯2019(高明的黑客强网先锋上单)
- 中国科学院沈阳计算技术研究所考研问题
- 登录Linux操作系统并执行第一个C语言代码
- mysql 拷贝表(复制表)的几种方式
- Nginx的搭建和优化
- 贾天昊 - Nick