项目实战——基于计算机视觉的物体位姿定位及机械臂抓取（双目标定）

        请各位读者朋友注意，这里面很多东西涉及到我的毕设，写作辛苦，请勿滥用，转载请务必注明出处！
        双目标定的目的是计算两个摄像机的相对位姿信息，并进行矫正，使得两台摄像机处于完全平行的状态。下面建立双摄像机模型，将相机相对位姿用数学表达式表达出来，并进行求解。

对极几何
        对极几何是双视图几何学理论的一部分，通常情况下，同一场景的两幅图像对应点之间存在着特殊的几何关系。这种关系仅取决于摄像机的内参数和两摄像机之间的相对位姿。

        1、极线约束

        对于单一的摄影机观测3D点w的情况，如图所示，w必定位于一条穿过光心和摄像机平面中 x 1的光线上。

   但是，并不能确定该点位于这条射线上的具体位置（图纸标记了四个可能的位置）。现在考虑观测同一个3D点的第二个摄像机，从第一个摄像机可得，该点必定位于3D空间中的一条特定光线上，因此第二幅图像中该点的投影位置 x 2 x_2 x 

​    
 必定唯一在第二幅图像中这条光线投影上的某个位置上。真实物理世界中的光线在二维平面中的投影称为极线。
        综上可以得出这样一个结论：对于第一幅图像中的任意点，它在第二幅图象中的对应点一定会在一条线上。这就是极线约束。该受约束的特定极线依赖于摄像机的内参数和外参数。

极线约束具有两个重要的实际意义：

        1) 在已知摄像机焦距、偏移量和相机间相对位姿的情况下，对于左视图中的二维像素点，只需在右摄像机拍摄图像中的极线进行一维搜索，即可得到该点在第二幅图像上的对应点位置。
        2) 对应点的约束是摄像机内在参数和外在参数的函数，已知摄像机内在参数的情况下，可利用对应点的观测模式来确定摄像机的外在参数，进而确定两台摄像机的相对位姿。

        2、极点

        在极线约束的基础上从另一个方面考量，第一幅图像中的每一个点都是物理世界中的一条光线的投影，而这些光线同时又在第二幅图像上投影形成极线。由于所有光线都聚集到摄像机1的光心，那么，所有光线在第二幅图像中形成的极线也将聚集于一点。该点即为摄像机1的光心在摄像机2中的对应点，也称极点。

        当然，极点并不一定在观测的图像内部，也有可能在图像之外的某一点上，下图描述了两种极端的情况：

 当两台摄像机位于一条直线上时，极点位于图像中心，且极线呈放射状分布；当两台摄像机都位于同一方向且垂直于光轴，此时极线都是互相平行的，而极点位于无穷远处。
        以上即为对极几何的主要内容，它对于计算摄像机相对位姿信息（求解摄像机外参数），进行立体匹配（寻找对应点）具有重要的作用。

基础矩阵与实矩阵
        为了研究方便，在研究双摄像机时，假定第一个摄像机为世界坐标的坐标中心，因此第一个摄像机的外在参数为：{I,0}。第二个摄像机处于任意位置。这样，在齐次坐标中，一个3D点投影到两个摄像机上，可表示为：

 造成八点算法不稳定的原因主要在于系数矩阵A， A直接使用的是原图像的像素坐标，没有经过任何处理，而每个像素点坐标分量的数量级相差太大，这就造成了矩阵A的不稳定。因此，在带入矩阵进行计算之前，首先要对像素坐标进行归一化处理，这样就可以消除数量级不同引起的巨大误差，提高矩阵A的稳定性。处理方法主要是对原始图像坐标进行同向性变换。
        归一化处理主要分为两步：
        1、变换选定像素点的位移，使得像素点集与原图像的原点重合；
        2、对图像点进行放缩变换，使他们到原点的平均距离为√2。

   上图形象地描述了归一化处理的过程：左侧为原始图像点，右侧为处理后的点，其中H为归一化变换的矩阵，可表示为：

  上述求得的F即为最终得到的基础矩阵。

        3、随机采样一致性（RANdom SAmple Consensus，RANSAC）

        通过两个摄像机的图像估计基础矩阵，唯一的输入来源就是匹配好的对应的点。在实际计算时，匹配点一定会存在误差，这种误差一般有以下两种情况:
        (1)测量点位置不精确引起的系统误差，这种误差服从高斯分布。
        (2)匹配点错误（错误的匹配点也称外点）引起的误差，这种误差不服从高斯分布。

        在计算的过程中，错误的匹配点能够造成极大的误差。对于外点，即使只有一组，也会造成很大的影响。因此，需要通过一种方法剔除外点，筛选出正确的匹配点（也称内点）。这种方法就是随机采样一致性（RANSAC）。

        该方法适用于外点较多时的情况（当外点较少时，可以采用最小二乘法来确定）。它的基本假设主要有两条：
        (1)数据中包含有内点，且内点数据符合某种数学分布；
        (2)外点并不符合内点的数学分布，一般是无规律的，其产生原因主要有：数据噪声、计算错误、测量错误等；

        下面给出RANSAC的算法：

        Step1：假设共有N个数据组成的集合P，集合P包含了内点和外点，并至少可以通过n个点拟合出模型；
        Step2：从集合P中随机选取n个点构成集合p；
        Step3：根据集合p估计出数学模型M；
        Step4：对剩余N-n个点分别判断是否适用模型M，适用的为内点，否则为外点，并统计内点的个数m_i。
        Step5：重复Step2~Step4步骤k次，并选取m_i最大（即:内点最多）的情况作为最终结果。

        该算法的核心是对于k值的选取。k值过小，则会对内点、外点估计错误；k值过大，则会增加计算工作量。实际上，可以通过理论计算来推断k值。Step2每次从集合P中选取n个点，假设选取到内点的概率为ϖ，则:
ϖ = 内 点 数 目 / 数 据 集 数 目 ϖ=内点数目/数据集数目 ϖ=内点数目/数据集数目

立体矫正
        通过计算出本征矩阵，并得到摄像机的相机转换矩阵后，根据2.1的基本原理，通过两个摄像机计算视深的前提是两个摄像机必须保证绝对的平行。因此下一步任务就是：使得两个摄像机的光轴完全平行，将摄像机所拍摄图像转换成完全平行摄像机所拍摄的图像。这一步也称为立体校正。

       在上图中，由于极线是相互平行的，因此三维世界的点在左右像面成的像高度是一致的。后面在进行立体匹配时，只需要在同一行上进行搜索即可，这样匹配效率就会大大提高。
本文采用Bouguet算法对图像进行立体校正，该算法的效果是：使左右视图中每幅图像的重投影畸变最小化，同时使视图的公共视野最大化。

主要思路是：将旋转矩阵R在两个摄像机之间分为两半（如果单纯旋转右侧摄像机与左侧平行，或者旋转左侧摄像机与右侧平行，那么它们的光轴方向就会发生很大的偏移，会使图像重投影畸变较大，因此折中的方法就是两侧都旋转一半，这样就可以最小化图像重投影畸变）。这里分别将左右摄像机的合成旋转矩阵称为 r l, r r 。将每个摄像机旋转二分之一，则它们的光轴最终平行于原始光轴所指的矢量和。需要注意的是，这样做会将两个摄像机设置为共面而非行对准。

        定义矩阵 R r e c t 为左摄像机的变换矩阵，目的是将其极线变为水平，且极点移至无穷远处。为了计算该矩阵，设图像主点 ( c x , c y ) 为原极点方向，则归一化后的极点方向为两台摄像机投影中心之间的平移矢量方向，这里构造一个由极点 ( e 1 ) ⃗ 方向开始的旋转矩阵

OpenCV双目标定
        上面详细阐述了双目标定的具体算法，下面详细阐述其具体操作方法。

       OpenCV提供了双目标定的实现。本文采用的C++语言开发，IDE为VS2015，OpenCV版本为4.0版本。以下介绍其主要实现函数：

        1、双目标定

        double cv :: stereoCalibrate （ 
        InputArrayOfArrays objectPoints，
        InputArrayOfArrays imagePoints1，
        InputArrayOfArrays imagePoints2，
        InputOutputArray cameraMatrix1，
        InputOutputArray distCoeffs1，
        InputOutputArray cameraMatrix2，
        InputOutputArray distCoeffs2，
        Size imageSize，
        OutputArray R，
        OutputArray T，
        OutputArray E，
        OutputArray F，
        INT flags = CALIB_FIX_INTRINSIC，
        TermCriteria standard =TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6)
        /*
        objectPoints 储存标定角点在世界坐标系中的位置；
        imagePoints1、imagePoints2 摄像机1、2的棋盘角点像素坐标；
        cameraMatrix1 左摄像机的内参矩阵；
        distCoeffs1 左摄像机的畸变系数矩阵；
        cameraMatrix2 右摄像机的内参矩阵；
        distCoeffs2 右摄像机的畸变系数矩阵；
        imageSize 图像的大小；
        R，左右摄像机的旋转矩阵；
        T，左右摄像机的平移矩阵；
        E，本征矩阵；
        F，基本矩阵；
        Flag 可能为零的不同标志或以下值的组合：
        CV_CALIB_FIX_INTRINSIC固定输入的cameraMatrix和distCoeffs不变，这样只估算R，T，E和F矩阵。
        CV_CALIB_USE_INTRINSIC_GUESS以单目标定的相机内参和畸变系数为初始值进行迭代。
        CV_CALIB_FIX_PRINCIPAL_POINT 不改变图像原点。
        CV_CALIB_FIX_FOCAL_LENGTH迭代过程中不会改变焦距
        CV_CALIB_ZERO_TANGENT_DIST不考虑两台摄像机的切向畸变系数。
        CALIB_THIN_PRISM_MODEL计算系数s1，s2，s3和s4。
        CALIB_TILTED_MODEL系数tauX和tauY已启用。
        /

      2、根据观察到的点坐标计算理想点坐标

        void cv :: undistortPoints （ 
        InputArray src，
        OutputArray dst，
        InputArray cameraMatrix，
        InputArray distcoeffs，
        InputArray R = noArray()，
        InputArray P =noArray()
        ） 
        /
        src， 观察到的点坐标
        dst，在非畸变和反向透视变换后输出理想点坐标。
        cameraMatrix 相机内参矩阵
        distCoeffs 相机畸变矩阵
        R - 对象空间中的整流变换（3x3矩阵），如果矩阵为空，则使用身份转换。
        P - 新的相机内参矩阵。
        /

      3、极线计算

        void cv :: computeCorrespondEpilines （ 
        InputArray points，
        // 输入矩阵。
        //包含点的图像索引（1或2）
        InputArray F，
        //基本矩阵
        OutputArray line
        //输出的极线参数矩阵
         ）; 

        4、基本矩阵计算

        Mat cv :: findFundamentalMat（ 
        InputArray points1，
        //第一幅图像的阵列点。
        InputArray points2，
        //与points1相同。
        Int method = FM_RANSAC，
        //用于计算基本矩阵的方法。
        /
        CV_FM_7POINT用于7点算法。ñ= 7
        CV_FM_8POINT通用的8点算法。ñ≥ 8
        用于RANSAC算法的        CV_FM_RANSAC。ñ≥ 8
        CV_FM_LMEDS为LMedS算法。ñ≥ 8
        /
        double param1 = 3.0，
        //用于随机一致性采样的参数。用于判断是否为噪点的阈值
        double param2 = 0.99，
        //用于随机一致性采样和LM算法的参数，表示置信水平
        OutputArray mask =noArray()
        ） 

        5、计算校准立体相机的每个摄像机的校正变换

        void cv :: stereoRectify （ 
        InputArray cameraMatrix1，
        InputArray distCoeffs1，
        InputArray cameraMatrix2，
        InputArray distCoeffs2，
        Size imageSize，
        InputArray R，
        InputArray T，
        OutputArray R1，
        OutputArray R2，
        OutputArray P1，
        OutputArray P2，
        OutputArray Q：
        Int flags = CALIB_ZERO_DISPARITY，
        double alpha = -1，
        Size newImageSize = Size()，
        Rect * validPixROI1 = 0，
        Rect * validPixROI2 =0
        ） 
        /
        cameraMatrix1– 第一个相机矩阵.
        distCoeffs1– 第一个相机畸变参数.
        cameraMatrix2
        distCoeffs2
        imageSize– 用于校正的图像大小；
        R–旋转矩阵；
        T– 平移矩阵；
        R1、R2–矫正旋转矩阵；
        P1、P2–投影矩阵（3x4）；
        Q–输出深度视差映射矩阵
        Flags-操作标志一般设为CV_CALIB_ZERO_DISPARITY，也可设置为0。
        Alpha-自由缩放参数。
        validPixROI1/ validPixROI2 -如果选择，则会输出矩形。
        /

       6、计算无畸变和裁剪后的转换图

        void cv :: initUndistortRectifyMap（ 
        InputArray cameraMatrix，
        InputArray distcoeffs，
        InputArray R，
        InputArray newCameraMatrix，
        Size size，
        Int m1type，
        OutputArray map1，
        OutputArray map2
        ）
        /
         cameraMatrix——摄像机内参矩阵
         distCoeffs——摄像机畸变系数矩阵
         R——两台摄像机之间的旋转矩阵
         newCameraMatrix——校正后的摄像机内参矩阵
         size——摄像机采集的无畸变图像尺寸
        m1type——map1的数据类型
        map1——输出的X坐标重映射参数
        map2——输出的Y坐标重映射参数
        */
        在本文程序中，双目标定写成了一个单独的.c文件，名称为StereoCalib，其主函数为：
        result *StereoCalib(
        int board_w, //棋盘的宽度
        int board_h, //棋盘的高度
        cv::Mat M1,
        cv::Mat D1,
         cv::Mat M2,
         cv::Mat D2,
         const char *imageList, //图像文件列表
        bool useUncalibrated, //是否使用未校准方法
        bool displayCorners, //是否显示角点
         bool showUndistores, //是否显示校正后的图像
         bool isVerticalStereo //判断图像是垂直还是水平
        )
        双目标定利用的是单目标定时所拍摄的图像，即:在单目标定时，当检测到棋盘图案时，左摄像机和右摄像机同时拍摄。经过双目标定，校正并对应后的图像如图所示:

 图像中包括左右摄像机拍摄棋盘图像经过单目标定、双目标定、裁剪后的对应图案，图中绿线即为极线（极点位于无穷远处），重映射平均误差为1.75537。
        经过计算，双目相机的R、T、E、F四个参数分别为：
        旋转矩阵（R）：

   
        PS:再次请各位读者朋友注意，这里面很多东西涉及到我的毕设，写作辛苦，请勿滥用，转载请务必注明出处！
————————————————
版权声明：本文为CSDN博主「Hunt Tiger Tonight」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_32061503/article/details/102825439