相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标

Posted Shawn · 视觉 数学 软件开发

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标相关的知识,希望对你有一定的参考价值。

关键词:相机位姿估计,单目尺寸测量,环境探知

用途:基于相机的环境测量,SLAM,单目尺寸测量

文章类型:原理说明、Demo展示

@Author:VShawn

@Date:2016-11-28

@Lab: CvLab202@CSU

目录

前言

早就写好了....不过doc放在笔记本电脑里,平时一直都在用台式机,所以拖到现在才发:(

 

写了这么多篇关于位姿估计的博客后,终于要写一篇有点用的东西了:本文将展示位姿估计的一种应用,即通过单目相机对环境进行测量。简单来说,本文的工作就是利用下面的两幅图,在已知P1、P2、P3、P4四点世界坐标的情况下,计算出P5的世界坐标。

该研究的应用范围很广,例如对某建筑群,可以通过设置几个已知的标志点(世界坐标已确定),用本文的方法将建筑的各个角的世界坐标求出来,从而测量出建筑的高度,建筑间的距离,乃至将整个建筑群的环境重构出来。又或者在某个露天货场,设置好标志点后只需要无人机飞一圈,就能知道货场中货物的体积有多少,从而安排货运计划。总之该项应用的前景很大,配合目前很火的无人机应用,可以为生产、研究带来不少的便利。最后,本文基于前几篇文章的结果,建议没有看过我博客的读者先读读前面的几篇原理介绍。

原理

在一开始,先设待求点为P

根据两条直线确定一个点的原理,在二维平面中只要知道两条相交直线的方程,就可以解出它们的交点坐标。现在假设我们是在二维平面中拍照的,如下图:

根据文章《相机位姿估计1:根据四个特征点估计相机姿态》的内容,我们根据P1、P2、P3、P4四点的空间坐标,可以估计出两次拍照的相机位姿Oc1与Oc2,也就知道了相机的坐标Oc1与Oc2。那么将Oc1与P,Oc2与P联成直线(如上图的橙色线),则可以获得两条直线方程,组成方程组求解得到它们的交点,即为待求点P的坐标。

到三维空间中,原理跟二维是一样的,只是两条直线从二维空间升到了三维空间成为了两条空间。通过解PNP求出了相机两次拍摄的空间位置Oc1、Oc2,在根据P在图像中的坐标,可以知道P点在空间中位于相机的哪个方向(将二维图像中的P点用公式映射到三维空间中,需要使用到内参数与外参数矩阵),也就是可以确定一条从相机指向点P的射线。用两幅图像确定了关于P的两条射线,那么解方程求出他们的交点坐标,就能得到P的空间坐标。

1.求出P点的相机坐标系坐标Pc

关于P点如何从二维映射到三维,参考上图,Oc的坐标通过解PNP已经求出,待求点P在图像中的像素坐标为(u,v)。根据《图像坐标系-相机坐标系-世界坐标系的关系》(由于懒癌,还没写)可以套公式求出P在相机坐标系中的坐标Pc(也就是上图中的Pc点)。具体的转换公式如下,式中F为相机镜头的焦距(mm),u、v为点的像素坐标,其余为相机内参数。

    

代码如下:

代码中使用本人封装好的解PNP问题类解决PNP问题,具体使用方法参见《OpenCV:solvePnP二次封装与性能测试》。

 

    PNPSolver p4psolver1;
    //初始化相机参数
    p4psolver1.SetCameraMatrix(fx, fy, u0, v0);
    //设置畸变参数
    p4psolver1.SetDistortionCoefficients(k1, k2, p1, p2, k3);

    p4psolver1.Points3D.push_back(cv::Point3f(0, 0, 0));        //P1三维坐标的单位是毫米
    p4psolver1.Points3D.push_back(cv::Point3f(0, 200, 0));        //P2
    p4psolver1.Points3D.push_back(cv::Point3f(150, 0, 0));        //P3
    p4psolver1.Points3D.push_back(cv::Point3f(150, 200, 0));    //P4
    //p4psolver1.Points3D.push_back(cv::Point3f(0, 100, 105));    //P5

    cout << "特征点世界坐标 = " << endl << p4psolver1.Points3D << endl << endl << endl;

    //求出图一中几个特征点与待求点P的坐标
    //cv::Mat img1 = cv::imread("1.jpg");
    p4psolver1.Points2D.push_back(cv::Point2f(2985, 1688));    //P1
    p4psolver1.Points2D.push_back(cv::Point2f(5081, 1690));    //P2
    p4psolver1.Points2D.push_back(cv::Point2f(2997, 2797));    //P3
    p4psolver1.Points2D.push_back(cv::Point2f(5544, 2757));    //P4
    //p4psolver1.Points2D.push_back(cv::Point2f(4148, 673));    //P5

    cout << "图一中特征点坐标 = " << endl << p4psolver1.Points2D << endl;

    cv::Point2f point2find1_IF = cv::Point2f(4149, 671);//图1中待求点P的图像坐标系坐标

    if (p4psolver1.Solve(PNPSolver::METHOD::CV_P3P) != 0)
        return -1;

    cout << "图一中相机位姿" << endl << "Oc坐标=" << p4psolver1.Position_OcInW << "      相机旋转=" << p4psolver1.Theta_W2C << endl;

    //将P投射到相机坐标系,再经过反旋转求出向量OcP,最终获得图1中,直线OcP上的两个点坐标,确定了直线的方程
    cv::Point3f point2find1_CF = p4psolver1.ImageFrame2CameraFrame(point2find1_IF, 350);//待求点P在图一状态下的相机坐标系坐标,输入参数350表示将P投影到350mm外的相机成像平面

2.求出P点在世界坐标系中的方向向量

此时我们得到了Pc(xc,yc,zc),但这个点坐标是在相机坐标系中的,而我们需要知道的其实是P点在世界坐标系中对应的坐标Pw(xw,yw,cw)。为了将Pc转为Pw,需要使用到解PNP求位姿时得到的三个欧拉角。我们知道相机坐标系C按照z轴、y轴、x轴的顺序旋转以上角度后将与世界坐标系W完全平行(详见《子坐标系C在父坐标系W中的旋转问题》),在这三次旋转中Pc显然是跟着坐标系旋转的,其在世界系W中的位置会随着改变。为了抵消旋转对P点的影响,保证C系旋转后P点依然保持在世界坐标系W原本的位置,需要对Pc进行三次反向旋转,旋转后得到点Pc在相机坐标系C中新的坐标值记为Pc\',Pc\'的值等于世界坐标系中向量OP的值。那么Pc\'的值+ Oc的世界坐标值=P点的世界坐标Pw。

代码如下(代码中变量接上一段代码):

 

    double Oc1P_x1 = point2find1_CF.x;//待求点P的相机坐标系x坐标
    double Oc1P_y1 = point2find1_CF.y; //待求点P的相机坐标系y坐标
    double Oc1P_z1 = point2find1_CF.z; //待求点P的相机坐标系z坐标
    //进行三次反向旋转,得到世界坐标系中向量OcP的值,也就是方向向量
    PNPSolver::CodeRotateByZ(Oc1P_x1, Oc1P_y1, p4psolver1.Theta_W2C.z, Oc1P_x1, Oc1P_y1);
    PNPSolver::CodeRotateByY(Oc1P_x1, Oc1P_z1, p4psolver1.Theta_W2C.y, Oc1P_x1, Oc1P_z1);
    PNPSolver::CodeRotateByX(Oc1P_y1, Oc1P_z1, p4psolver1.Theta_W2C.x, Oc1P_y1, Oc1P_z1);
    //两点确定一条直线,a1为Oc的世界坐标,a2为P的世界坐标Pw
    cv::Point3f a1(p4psolver1.Position_OcInW.x, p4psolver1.Position_OcInW.y, p4psolver1.Position_OcInW.z);
    cv::Point3f a2(p4psolver1.Position_OcInW.x + Oc1P_x1, p4psolver1.Position_OcInW.y + Oc1P_y1, p4psolver1.Position_OcInW.z + Oc1P_z1);

 上面的代码中获得了一条射线A的两个端点,其中a1为相机的世界坐标系坐标,a2为求出的P点映射到世界坐标系时的方向向量+相机的世界坐标系坐标

3.最后,根据两幅图得到的两条直线,计算出P点的世界坐标

对另外一幅图也进行1、2的操作,得到点b1,b2。于是获得两条直线A、B,求出两条直线A与B的交点,大功告成。然而在现实中,由于误差的存在,A与B相交的可能性几乎不存在,因此在计算时,应该求他们之间的最近点坐标。

根据文章《求空间内两条直线的最近距离以及最近点的坐标(C++)》中给出的GetDistanceOf2linesIn3D类,可以求出两条直线的交点或者说两条直线的最近点坐标。

代码:

    /*************************求出P的坐标**************************/
    //现在我们获得了关于点P的两条直线a1a2与b1b2
    //于是两直线的交点便是点P的位置
    //但由于存在测量误差,两条直线不可能是重合的,于是退而求其次
    //求出两条直线最近的点,就是P所在的位置了。

    GetDistanceOf2linesIn3D g;//初始化
    g.SetLineA(a1.x, a1.y, a1.z, a2.x, a2.y, a2.z);//输入直线A上的两个点坐标
    g.SetLineB(b1.x, b1.y, b1.z, b2.x, b2.y, b2.z);//输入直线B上的两个点坐标
    g.GetDistance();//计算距离
    double d = g.distance;//获得距离
    //点PonA与PonB分别为直线A、B上最接近的点,他们的中点就是P的坐标
    double x = (g.PonA_x + g.PonB_x) / 2;
    double y = (g.PonA_y + g.PonB_y) / 2;
    double z = (g.PonA_z + g.PonB_z) / 2;

    cout << endl << "-------------------------------------------------------------" << endl;
    cout << "解得P世界坐标 = (" << x << "," << y << "," << z << ")" << endl;

  

结果

最后解得P点坐标为:

P的实际坐标为(5,100,105),计算结果的误差在1mm左右,考虑到绘图与测量时产生的误差,以及拍摄的时的视距,这样的误差在可接受范围之内。

应用

将上述理论应用到实际当中,我用130w的工业相机在距离800上对目标拍摄了一系列的图。

 

误差分析

该测量的误差来源于以下几个方面:

  1. 安装、测量误差

    这个误差是由于在设备安装,以及尺寸测量中所形成的。最典型的比如说在用马克笔画点时画歪了一点,又或在用尺子测量P5点高度时度数不准等。

  2. 像点坐标的选取误差

    这一误差是在确定几个点的像素坐标时,取点不准所造成的。不过由于本文用的图像有2000w像素,因此该误差不太明显。若是用100w的图像,该误差的影响就会被大大增强。

  3. 两张拍照位置造成的误差。

    理论上两次拍照位置相互垂直时,最后计算出来的P点世界坐标的误差最小,如下图。

但实际情况却不一定这么理想,尤其是在处理无人机拍摄的视频时,可能隔几帧就进行一次处理,这样就会带来较大的误差,所以最好的办法是求多组值,取它们的加权平均值。像本文所用的两幅图像的拍摄角度大概是这样的:

主视图:

侧视图:

用这两幅图像处理产生的误差就会比较大,但最终计算出的误差也就在1mm左右,能够接受。用这两幅图做实验是因为作者比较懒惰,直接用了以前拍的图片,而没有重新采集图像,好同志不要学。

以上是关于相机位姿估计3:根据两幅图像的位姿估计结果求某点的世界坐标的主要内容,如果未能解决你的问题,请参考以下文章

相机位姿求解——P3P问题

通过单应性或solvePnP()函数估计相机位姿

相机位姿估计2:[应用]实时位姿估计与三维重建相机姿态

相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试

使用单相机进行标记姿态估计的误差

3D-2D:PnP