位姿估计_3
来源:互联网 发布:行政审批优化 编辑:程序博客网 时间:2024/04/28 09:55
这里是位姿估计的第三讲,利用灭点(vanishing point)来估计相机位姿。
灭点 vanishing point
灭点的介绍,wiki百科,简单的说,就是在真是物理世界中相互平行的两条直线,在相机的2d投影中,会汇聚相交到一点,该点就是灭点或者消失点(vanishing point),抽象描述物理世界中的无穷远处。
如图所示
图中的灰色部分表示一个十字路口,其中在Z方向的无穷远处有一点Z_inf,其在真实世界中的坐标为Z_inf = [0, 0, 1, 0]。我的理解是,在Z方向的无穷远处,X和Y是远小于Z的,可以忽略为零,而且在该坐标的表达方式上,w=0,也表示该点位于Z的无穷远处。
在十字路口在相机的2D投影中,可以看到无穷远处的路两边是相交到一点了,这就是灭点,其中Zv对应Z_inf点
这里旋转矩阵R=[r1,r2,r3],内参矩阵为K。
根据相机成像原理有
z*Vz = K[r1, r2, r3 | t]Z_inf, 即 z*Vz = K[r1, r2, r3 | t][0, 0, 1, 0]T,如下
化简可得
z*Vz = Kr3, ==> r3 = (k逆)Vz / ||(k逆)Vz||,如下
利用沿着z方向的灭点可以求出旋转矩阵的r3,同理,利用沿着x方向的灭点可以求出旋转矩阵的r1,沿着y方向的灭点可以求出旋转矩阵的r2,但是在求旋转矩阵的时候只需要知道两个方向的r即可,第三个方向上的r可以通过已知两个方向上的r叉乘得到
如下
利用灭点求解位姿步骤
- 准备一张比较好的图片
- 分别找出两组平行线上的两个点,计算得出灭点
- 结合相机内参矩阵,计算分别得到两个方向上的r,并归一化
- 两个互相垂直的r叉乘得到另外一个维度上的r
至此就得到了旋转矩阵R,但是这里依然得不到平移向量t,因为仅靠旋转是没有办法得出的
结果
教程上的求解灭点展示
自己求解灭点展示
疑惑
使用灭点求解的旋转矩阵R怎么和使用arcuo库函数得到的结果不一样呐,理解的朋友帮忙指点一下吧,谢谢!!!
代码
//这里的cornerReturn是vector<Point2f>类型的,包含四个元素,分为两组,是两条垂直直线上的点 Point2f vanish_y; float ky1=0, ky2=0; ky1 = (cornerReturn[0].y - cornerReturn[3].y)/(cornerReturn[0].x - cornerReturn[3].x); ky2 = (cornerReturn[1].y - cornerReturn[2].y)/(cornerReturn[1].x - cornerReturn[2].x); cout<<"ky is "<<ky1<<" "<<ky2<<endl; vanish_y.x = (ky1*cornerReturn[0].x - cornerReturn[0].y - ky2*cornerReturn[1].x +cornerReturn[1].y)/(ky1 - ky2); vanish_y.y = ky1*(vanish_y.x - cornerReturn[0].x) + cornerReturn[0].y; line(frame, cornerReturn[0], vanish_y, Scalar(0, 0, 255), 2); line(frame, cornerReturn[1], vanish_y, Scalar(0, 0, 255), 2); cout<<"y vanishing point is "<<vanish_y<<endl; //x vanishing point Point2f vanish_x; float kx1=0, kx2=0; kx1 = (cornerReturn[0].y - cornerReturn[1].y)/(cornerReturn[0].x - cornerReturn[1].x); kx2 = (cornerReturn[2].y - cornerReturn[3].y)/(cornerReturn[2].x - cornerReturn[3].x); cout<<"kx is "<<kx1<<" "<<kx2<<endl; vanish_x.x = (kx1*cornerReturn[0].x - cornerReturn[0].y - kx2*cornerReturn[3].x +cornerReturn[3].y)/(kx1 - kx2); vanish_x.y = kx1*(vanish_x.x - cornerReturn[1].x) + cornerReturn[1].y; line(frame, cornerReturn[0], vanish_x, Scalar(255, 0, 0), 2); line(frame, cornerReturn[3], vanish_x, Scalar(255, 0, 0), 2); cout<<"x vanishing point is "<<vanish_x<<endl; Mat Vx = (Mat_<float>(3, 1) << vanish_x.x, vanish_x.y, 1.f); Mat Vy = (Mat_<float>(3, 1) << vanish_y.x, vanish_y.y, 1.f); Mat r1 = Mat(3,1,CV_32F); Mat r2 = Mat(3,1,CV_32F); Mat kInv = intrinsic_matrixCommon.inv(); cout<<"kInv is "<<kInv<<endl; float normX; Mat rTemp1 = kInv*Vx; normX = abs(rTemp1.at<float>(0,0)) + abs(rTemp1.at<float>(1,0)) + abs(rTemp1.at<float>(2,0)); cout<<"rTemp1 is "<<rTemp1<<endl<<"normX is "<<normX<<endl; //第一个方向上的r,并归一化 r1 = rTemp1/normX; cout<<"r1 is "<<r1<<endl; cout<<"******************* "<<endl; float normY; Mat rTemp2 = kInv*Vy; normY = abs(rTemp2.at<float>(0,0)) + abs(rTemp2.at<float>(1,0)) + abs(rTemp2.at<float>(2,0)); cout<<"rTemp2 is "<<rTemp2<<endl<<"normY is "<<normY<<endl; //第二个方向上的r,并归一化 r2 = rTemp2/normY; cout<<"r2 is "<<r2<<endl; Mat r3 = Mat(3,1,CV_32F); //叉乘得到第三个方向上的r r3 = r1.cross(r2); cout<<"r3 is "<<r3<<endl; cout<<"******************* "<<endl; //这是得到的旋转矩阵 Mat rotateMatrix = Mat(3,3,CV_32F); rotateMatrix.at<float>(0,0) = r1.at<float>(0,0);//x rotateMatrix.at<float>(1,0) = r1.at<float>(1,0); rotateMatrix.at<float>(2,0) = r1.at<float>(2,0); rotateMatrix.at<float>(0,1) = r2.at<float>(0,0);//y rotateMatrix.at<float>(1,1) = r2.at<float>(1,0); rotateMatrix.at<float>(2,1) = r2.at<float>(2,0); rotateMatrix.at<float>(0,2) = r3.at<float>(0,0);//z rotateMatrix.at<float>(1,2) = r3.at<float>(1,0); rotateMatrix.at<float>(2,2) = r3.at<float>(2,0); cout<<"rotateMatrix is "<<endl<<rotateMatrix<<endl;
- 位姿估计_3
- 位姿估计_1
- 位姿:***图像特征-特征提取-姿态估计
- PCL之空间非合作目标位姿估计
- 一种简单的位姿估计(PoseEstimation)计算方法
- 位姿:***图像特征-特征提取-姿态估计
- 位姿:***图像特征-特征提取-姿态估计
- ORB-SLAM2跟踪之估计初始位姿
- 相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标
- 相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标_0
- 基于VFH描述子的聚类识别与6自由度位姿估计的学习
- PnP 单目相机位姿估计(一):初识PnP问题
- PnP 单目相机位姿估计(二):solvePnP利用二维码求解相机世界坐标
- PnP 单目相机位姿估计(三):二维码角点检测
- Brown-Mood估计、Theil估计 2、Siegel估计、线性分位回归的r语言代码
- 黑马程序员_3 C语言基础-函数-进制-位运算
- 估计
- 估计
- ImageMagick 命令执行漏洞
- Json基础解释
- 共同学习Java源代码-多线程与并发-CompletionService接口和ExecutorCompletionService类
- 程序员必须知道的十大基础实用算法及其讲解
- yii2速查手册
- 位姿估计_3
- JAVA反射
- kafka性能测试
- 解决使用jpa的时候set实体类属性自动持久化
- Struts2远程命令执行漏洞
- Python之道1-环境搭建与pycharm的配置django安装及MySQL数据库配置
- 2017需要关注的论文
- Ubuntu的目录和权限(hadoop学习笔记)
- 代码审计