【计算机视觉】opencv姿态解算4 视觉导航单目特征检测与实时位姿

2024-05-20 01:58:50

转载关于OpenCV的那些事——相机姿态更新

上一节我们使用张正友相机标定法获得了相机内参，这一节我们使用 PnP (Perspective-n-Point)算法估计相机初始姿态并更新之。推荐3篇我学习的博客：【姿态估计】Pose estimation algorithm 之 Robust Planar Pose (RPP)algorithm，POSIT算法的原理--opencv 3D姿态估计，三维姿态：关于solvePnP与cvPOSIT。

2016/6/20

关于PnP问题我会重新写一篇博客，讲一下概念，最少需要几组对应的3D/2D点，

3D点共面时怎么处理，PnP有哪些主流解法，以及会更新一篇G2O的PnP解法。

/

注意点1：solvePnP里有三种解法：P3P， EPnP，迭代法（默认）；opencv2里参数分别为CV_P3P，CV_EPNP，CV_ITERATIVE （opencv3里多了DLS和UPnP解法）。

注意点2：solvePnP需要至少3组点：P3P只使用4组点，3组求出多个解，第四组确定最优解；EPnP使用大于等于3组点；迭代法调用cvFindExtrinsicCameraParams2，进而使用SVD分解并调用cvFindHomography，而cvFindHomography需要至少4组点。

接下来我们使用OpenCV实现相机姿态更新：

上一节得到的相机内参和相机畸变：

[cpp] view plaincopy print?

double camD[9] = {618.526381968738, 0, 310.8963715614199,
0, 619.4548980786033, 248.6374860176724,
0, 0, 1};
double distCoeffD[5] = {0.09367405350511771, -0.08731677320554751, 0.002823563134787144, -1.246739177460954e-005, -0.0469061739387372};
Mat camera_matrix = Mat(3,3,CV_64FC1,camD);
Mat distortion_coefficients = Mat(5,1,CV_64FC1,distCoeffD);

首先检测ORB角点并亚像素化：

[cpp] view plaincopy print?

cap >> frame;
if( frame.empty() )
break;
frame.copyTo(image);
if(needToGetgf)
{
cvtColor(image, gray, COLOR_BGR2GRAY);
// automatic initialization
orb.detect(gray, keypoints);
goodfeatures.clear();
for( size_t i = 0; i < keypoints.size(); i++ ) {
goodfeatures.push_back(keypoints[i].pt);
}
cornerSubPix(gray, goodfeatures, subPixWinSize, Size(-1,-1), termcrit);
for(size_t i = 0; i < goodfeatures.size(); i++ )
{
circle( image, goodfeatures[i], 3, Scalar(0,255,0), -1, 8);
}
}

使用鼠标选定4个2D点（按正方形左上顶点开始顺时针），然后查找所选点附近的角点，若找到则压入跟踪点集合：

[cpp] view plaincopy print?

void on_mouse(int event,int x,int y,int flag, void *param)
{
if(event==CV_EVENT_LBUTTONDOWN)
{
if(needtomap && points[1].size()<4)
{
for(size_t i = 0;i<goodfeatures.size();i++)
{
if(abs(goodfeatures[i].x-x)+abs(goodfeatures[i].y-y)<3)
{
points[1].push_back(goodfeatures[i]);
trackingpoints++;
break;
}
}
}
}
}

建立与2D跟踪点集合相对应的3D空间点集合：

[cpp] view plaincopy print?

objP.push_back(Point3f(0,0,0)); //三维坐标的单位是毫米
objP.push_back(Point3f(5,0,0));
objP.push_back(Point3f(5,5,0));
objP.push_back(Point3f(0,5,0));
Mat(objP).convertTo(objPM,CV_32F);

使用LK光流法跟踪已选定角点：

[cpp] view plaincopy print?

vector<uchar> status;
vector<float> err;
if(prevGray.empty())
gray.copyTo(prevGray);
calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err);
size_t i,k;
for(i = k = 0; i < points[1].size(); i++ )
{
if( !status[i] )
continue;
points[1][k++] = points[1][i];
circle( image, points[1][i], 3, Scalar(0,0,255), -1, 8);
}

若4个点均跟踪成功，使用solvePnP计算相机姿态，并使用计算出的相机姿态重画3D空间点到2D平面查看是否匹配：

[cpp] view plaincopy print?

if(k == 4)
getPlanarSurface(points[0]);

[cpp] view plaincopy print?

void getPlanarSurface(vector<Point2f>& imgP){
Rodrigues(rotM,rvec);
solvePnP(objPM, Mat(imgP), camera_matrix, distortion_coefficients, rvec, tvec);
Rodrigues(rvec,rotM);
cout<<"rotation matrix: "<<endl<<rotM<<endl;
cout<<"translation matrix: "<<endl<<tv[0]<<" "<<tv[1]<<" "<<tv[2]<<endl;
projectedPoints.clear();
projectPoints(objPM, rvec, tvec, camera_matrix, distortion_coefficients, projectedPoints);
for(unsigned int i = 0; i < projectedPoints.size(); ++i)
{
circle( image, projectedPoints[i], 3, Scalar(255,0,0), -1, 8);
}
}

通过查看cmd中输出的旋转矩阵和平移向量以及重画的2D点，我们发现solvePnP运行良好。点这里获得程序源码

下一节我们将结合相机外参使用OpenGL画出AR物体。

============================================================================================================

2015/10/20号补充：

这几天在做跟踪恢复的时候需要用给定的2D点和R，T计算3D点，于是重新手算了一边图像2D点和空间3D点的关系。过程中搞懂了为什么PnP计算rotation和translation的时候需要至少3组2D/3D点。

首先来看图像2D点和空间3D点的关系：

对于R和T展开并且对矩阵相乘展开我们得到：

把（3）式带入（1）式和（2）式，整理得：

Xw * ( fx * R11 + u0 * R31 - x * R31) + Yw * (fx * R12 + u0 * R32 - x * R32) + Zw * (fx * R13 + u0 * R33 - x * R33) = T3 * x - fx * T1 - u0 * T3

Xw * ( fy * R21 + v0 * R31 - y * R31) + Yw * (fy * R22 + v0 * R32 - y * R32) + Zw * (fy * R23 + v0 * R33 - y * R33) = T3 * y - fy * T2 - v0 * T3

我们可以看出，fx fy u0 v0是相机内参，上一节中已经求出，Xw Yw x y是一组3D/2D点的坐标，所以未知数有R11 R12 R13 R21 R22 R23 R31 R32 R33 T1 T2 T3一共12个，由于旋转矩阵是正交矩阵，每行每列都是单位向量且两两正交，所以R的自由度为3，秩也是3，比如知道R11 R12 R21就能求出剩下的Rxx。加上平移向量的3个未知数，一共6个未知数，而每一组2D/3D点提供的x y Xw Yw Zw可以确立两个方程，所以3组2D/3D点的坐标能确立6个方程从而解出6个未知数。

故PnP需要知道至少3组2D/3D点。

============================================================================================================

2016/1/28号补充：

最近在用平均最小误差求精准相机姿态的过程中，需要搞清楚R和t的具体含义。

R的第i行表示摄像机坐标系中的第i个坐标轴方向的单位向量在世界坐标系里的坐标；
R的第i列表示世界坐标系中的第i个坐标轴方向的单位向量在摄像机坐标系里的坐标；
t 表示世界坐标系的原点在摄像机坐标系的坐标；
-R的转置 * t 表示摄像机坐标系的原点在世界坐标系的坐标。（原理如下图，t表示平移，T表示转置）

2016/6/20

关于PnP问题我会重新写一篇博客，讲一下概念，最少需要几组对应的3D/2D点，

3D点共面时怎么处理，PnP有哪些主流解法(P3P, EPnP, DLS, UPnP, 传统迭代)，

以及会更新一篇G2O的PnP解法（传统迭代，最小化重投影误差）。

/

【计算机视觉】opencv姿态解算4 视觉导航单目特征检测与实时位姿相关推荐

【计算机视觉】opencv姿态解算6 理论算法调研 PNP问题 5种算法
关于PnP(pespective-n-point)的一些方法最小PnP问题 P3P问题中假设没有噪声,使用几何约束,可以解得相机的位姿.不具有唯一解. P4P问题中分为线性方法和基于P3P的方法. ...
【计算机视觉】opencv姿态解算7 四大坐标系,摄像机线性模型, 畸变模型
转载 http://blog.csdn.net/tiemaxiaosu/article/details/51725240 1 坐标系统四大坐标系一.成像坐标 (1).图像坐标系(Pixel coo ...
Pixhawk代码分析-姿态解算篇C
姿态解算篇C 前言终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然 ...
Pixhawk之姿态解算篇（3）_源码姿态解算算法分析
一.开篇终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然后再做什么 ...
树莓派采集MPU9250运行AHRS进行姿态解算
文章目录 1.几种概念的区分 2.消费级IMU的AHRS 3.树莓派玩转MPU9250 3.1树莓派配置 3.2在树莓派中移植MPU9250库 3.3使用MPU9250库 4.校准 4.1IMU误差模 ...
单目相机三维姿态解算
单目相机三维姿态解算 Abstract:This passage mainly describes how to solve pose(Yaw,Pitch,Roll)with signal camer ...
PX4姿态解算源码原理理解
PX4源码原理理解一．主要参考资料链接:1.1 取PX4源码一小部分姿态解算来进行讲解姿态解算源码中文注释:https://blog.csdn.net/zouxu634866/article/deta ...
四旋翼姿态解算——梯度下降法理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...

最新文章

热门文章