精华内容
下载资源
问答
  • 二维平面坐标系转换

    2021-04-25 14:26:03
    二维平面坐标系转换 在运动控制中,常涉及到物料坐标系与载台坐标系之间的转换。 通常用坐标转换矩阵实现同一点在不同坐标系的转换,即指将两个点之间的仿射关系以转换矩阵的形式表现出来,转换矩阵作用于原始点的...

    二维平面坐标系转换

    在运动控制中,常涉及到物料坐标系与载台坐标系之间的转换。
    通常用坐标转换矩阵实现同一点在不同坐标系的转换,即指将两个点之间的仿射关系以转换矩阵的形式表现出来,转换矩阵作用于原始点的坐标,得到相对应的通过(平移,缩放,旋转)得到的新的点的坐标。

    平移:

    点P(x,y)平移得到点P'(x',y'),

    矩阵表示:

    缩放:

    矩阵表示:


    旋转:

    矩阵表示:


    旋转证明:

    点P向逆时针旋转了θ度,坐标系如下图,

    依图可知,

    其中的旋转角度θ求法:已知对应坐标系下的两点坐标,求两向量的夹角,两向量的夹角的cos值等于两向量的数量积除以两向量的模的乘积

    假设,已知物料坐标系中两点坐标p1(x1,y1),p2(x2,y2),其两点在载台坐标系中的坐标为p3(x3,y3),p4(x4,y4),那么

    坐标转换矩阵

    如果一个点同时进行了平移,缩放,旋转,用以下坐标变换矩阵:

    即就是a,b,c,d为缩放、旋转的系数,e,f为平移量


    eg:
    物料坐标系与载台坐标系如下:


    先旋转物料坐标系180°,使其与载台坐标系一致:

    C#实现坐标转换矩阵:

    using MathNet.Numerics.LinearAlgebra;
    using MathNet.Numerics.LinearAlgebra.Double;
    class CoorTransform
        {
            public CoorTransform() { }
    
            public double Mark1_cust_position_x { get; set; }
            public double Mark1_cust_position_y { get; set; }
            public double Mark2_cust_position_x { get; set; }
            public double Mark2_cust_position_y { get; set; }
            public double Mark1_stage_position_x { get; set; }
            public double Mark1_stage_position_y { get; set; }
            public double Mark2_stage_position_x { get; set; }
            public double Mark2_stage_position_y { get; set; }
            Matrix<double> matrix_rotation { get; set; }   //转换矩阵
    
            // 设置转换矩阵矩阵   如不同坐标系应先完成坐标系转换(这里的不同指左手坐标系和右手坐标系)
            public void SetMatrix()
            {
                double s1_x = Mark2_cust_position_x - Mark1_cust_position_x;
                double s1_y = Mark2_cust_position_y - Mark1_cust_position_y;
    
                double s2_x = Mark2_stage_position_x - Mark1_stage_position_x;
                double s2_y = Mark2_stage_position_y - Mark1_stage_position_y;
    
    
    
                double k = 1;
    
                var vectorA = new DenseVector(new[] { s1_x, s1_y });
    
                var vectorB = new DenseVector(new[] { s2_x, s2_y });
    
    
                double d = vectorA * vectorB;
                //两向量的夹角的cos值=向量的数量积/向量模的乘积    向量的夹角即为两坐标系间的旋转角度
                var cos_sg = (vectorA * vectorB) / (Math.Sqrt(s1_x * s1_x + s1_y * s1_y) * System.Math.Sqrt(s2_x * s2_x + s2_y * s2_y));
    
                var sin_sg = Math.Sin(Math.Acos(cos_sg));
    
                //缩放比例
                k = Math.Sqrt((s2_x * s2_x + s2_y * s2_y) / (s1_x * s1_x + s1_y * s1_y));
    
                if (((s1_y / s1_x) > (s2_y / s2_x)) & (s1_x > 0))
                {
                    sin_sg = -sin_sg;
                }
    
    
                double[,] stage2 =
               {
    
                    { k*cos_sg, k*-sin_sg},
                    { k*sin_sg,k*cos_sg},
                };
    
                matrix_rotation = DenseMatrix.OfArray(stage2);
    
                //两坐标系间的偏移
                double offset_x = Mark1_stage_position_x - Mark1_cust_position_x;
                double offset_y = Mark1_stage_position_y - Mark1_cust_position_y;
    
    
                double[,] stage1 =
                {
                    { k*cos_sg, k*-sin_sg,offset_x},
                    { k*sin_sg,k*cos_sg,offset_y},
                    { 0,0,1}
                };
    
                matrix_rotation = DenseMatrix.OfArray(stage1);
            }
    
            public void Check(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4,double x5, double y5)
            {
                Mark1_cust_position_x = x1;
                Mark1_cust_position_y = y1;
                Mark2_cust_position_x = x2;
                Mark2_cust_position_y = y2;
                Mark1_stage_position_x = x3;
                Mark1_stage_position_y = y3;
                Mark2_stage_position_x = x4;
                Mark2_stage_position_y = y4;
    
                SetMatrix();
                //由于物料坐标系做了旋转变换(180°),故此坐标点均乘以-1,即旋转矩阵(-1  0
                //                                                           0  -1)
                var vectorsssss = new DenseVector(new[] { -x5, -y5, 1 });
                var result = matrix_rotation * vectorsssss;
            }
        }

     

     

    参考:https://blog.csdn.net/Xu_Claire/article/details/101477540

    展开全文
  • 1.像素坐标系、像平面坐标希、相机坐标系、世界坐标系的定义;2.四个坐标系之间的相互转换关系

    【图像处理】像素坐标系、像平面坐标系、相机坐标系、世界坐标系、内参矩阵、外参矩阵

    像素坐标系

    以图像左上角为原点建立以像素为单位的直接坐标系uvu-v。横轴为uu,向右为正方向;纵轴为vv,向下为正方向
    uuvv代表像素的行数和列数,没有物理单位
    在这里插入图片描述

    像平面坐标系

    像平面坐标系oxyo-xy是以影像几何中心oo为原点,xxyy轴方向分别为平行于影像画幅边缘线的二维坐标系
    在这里插入图片描述

    相机坐标系

    相机坐标系ocxcyczco_c-x_cy_cz_c是以摄像机光心为原点(在针孔模型中也就是针孔为光心),z轴与光轴重合也就是z轴指向相机的前方(也就是与成像平面垂直),x轴与y轴的正方向与像平面坐标系平行的三维直角坐标系

    注意下图不符合针孔成像,只是示意图。如果是针孔成像,z轴方向与下图相反
    在这里插入图片描述

    世界坐标系

    由于摄像机可安放在环境中的任意位置,在环境中选择一个基准坐标系来描述摄像机的位置,并用它描述环境中任何物体的位置,该坐标系称为世界坐标系owxwywzwo_w-x_wy_wz_w
    相机坐标系与世界坐标系之间的关系可以用旋转矩阵与平移向量来描述

    像素坐标系<=>像平面坐标系

    假设(u0,v0)(u_0,v_0)代表O1O_1uvu-v坐标系下的坐标,dxd_xdyd_y分别表示每个像素在像平面坐标系横轴xx和纵轴yy上的物理尺寸
    则像素坐标系和像平面坐标系有如下关系:
    在这里插入图片描述
    写成矩阵形式,就是:
    在这里插入图片描述

    像素坐标系<=>相机坐标系

    在这里插入图片描述更正:
    上图中红色的不是像平面坐标系,而是原点从左上平移到中心的像素坐标系!

    写成矩阵形式,就是:
    在这里插入图片描述

    相机坐标系<=>世界坐标系

    世界坐标系是为了描述相机的位置而被引入的
    平移向量tt和旋转矩阵RR可以用来表示相机坐标系与世界坐标系的关系
    所以,假设空间点PP在世界坐标系下的齐次坐标是(xw,yw,zw,1)T(x_w, y_w, z_w, 1)^T,在相机坐标系下的齐次坐标是(xc,yc,zc,1)T(x_c, y_c, z_c, 1)^T,则存在如下关系:
    在这里插入图片描述

    结语

    如果您有修改意见或问题,欢迎留言或者通过邮箱和我联系。
    手打很辛苦,如果我的文章对您有帮助,转载请注明出处。

    展开全文
  • 坐标系转换 世界坐标系 由于摄像机与被摄物体可以放置在环境中的任意位置,这样就需要在环境中建立一个坐标系,来表示摄像机和被摄物体的位置,这个坐标就称为世界坐标系 相机坐标系 也是一个三维直角坐标系,原点...

    最近在做传感器融合方面的研究,但相关的资料甚少,想着分解一块一块地实现(补充知识),总之道阻且长,一步步来叭

    坐标系转换

    世界坐标系

    由于摄像机与被摄物体可以放置在环境中的任意位置,这样就需要在环境中建立一个坐标系,来表示摄像机和被摄物体的位置,这个坐标就称为世界坐标系

    相机坐标系

    也是一个三维直角坐标系,原点位于镜头的光心处,x,y轴分别与相面的两边平行,z轴为镜头的光轴,与像平面垂直

    世界坐标系到相机坐标系

    旋转

    旋转的表示

    • 旋转矩阵
    • 欧拉角
    • 四元数
    • 轴角
    • 李群与李代数

    旋转的应用场景

    惯性导航 机器人学 (机械臂,无人机,SLAM)

    步骤

    1. 先旋转对齐
      在这里插入图片描述
      [xcyczc]=[1000cosθsinθ0sinθcosθ][xyz]\begin{bmatrix} x_{c} \\ y_{c} \\ z_{c} \end{bmatrix}=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \theta & \sin \theta \\ 0 & -\sin \theta & \cos \theta \end{bmatrix}\begin{bmatrix} x \\ y \\ z \end{bmatrix}

    具体推导
    在这里插入图片描述

    {Xc=XYc=cosθY+sinθZZc=sinθY+cosθZ\begin{cases}X_{c}=X\\ Y_{c}=\cos \theta \cdot Y+\sin \theta \cdot Z\\ Z_{c}=-\sin \theta \cdot Y+\cos \theta \cdot Z\end{cases}

    所有的分解都满足欧式距离不变性

    其他旋转矩阵

    • 绕X轴
      R(XA,θ)=[1000cosθsinθ 0sinθcosθ]R\left( X_{A},\theta \right) =\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ \ 0 & \sin \theta & \cos \theta \end{bmatrix}
    • 绕Y轴
      R(YA,θ)=[cosθ0sinθ010 sinθ0cosθ]R\left( Y_{A},\theta \right) =\begin{bmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ \ -\sin \theta & 0 & \cos \theta \end{bmatrix}
    • 绕Z轴
      R(ZA,θ)=[cosθsinθ0sinθcosθ0 001]R\left( Z_{A},\theta \right) =\begin{bmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ \ 0 & 0 & 1 \end{bmatrix}

    把三个绕不同方向的旋转进行相乘

    在这里插入图片描述
    这样可以表示空间中任意方向中的旋转

    左乘还是右乘

    • 左乘:相对于固定坐标系进行变换
      V=R×V=(RZ×RY×RX)×VV'=R\times V=\left( R_{Z}\times R_{Y}\times R_{X}\right) \times V
      对每个固定的坐标系进行变换
    • 右乘:相对于自身(活跃/变化)的坐标系进行变换
      每一次变换后按新的(自身的)坐标系进行变换
      详细参考这篇文章
      旋转的左乘与右乘

    平移

    我们把旋转矩阵称为R,T为X,Y,Z三轴方向的平移,(R,T即为相机的外参)
    [XcYcZc]=R[XwYwZw]+T\begin{bmatrix} X_{c} \\ Y_{c} \\ Z_{c} \end{bmatrix}=R\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \end{bmatrix}+T

    =[r11r12r13r21r22r23r31r32r33][XwYwZw]+[txtytz]=\begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \end{bmatrix}+\begin{bmatrix} tx \\ ty \\ tz \end{bmatrix}

    齐次方程形式
    [xcyczc1]=[Rt01][xwywzw1]\begin{bmatrix} x_{c}\\ y_{c} \\ z_{c} \\ 1 \end{bmatrix}=\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}\begin{bmatrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{bmatrix}

    总结

    1. 世界坐标系变换到相机坐标系,是为了将像素点与世界点联系起来
    2. 世界点:我们需要待测量物体到机器人的距离和位置关系,世界坐标系一般定在机器人上,或者是机器人工作的场景之中
    3. 世界坐标系与相机坐标系的关系就是相机的外参
    展开全文
  • 注意是世界坐标系Z=0的平面 按照我的理解,是不是对像素图片进行单应性矩阵H的逆变换?如果是这样,单应性矩阵H怎么根据内参矩阵、畸变参数、旋转平移矩阵求出?张正友标定法里的单应性矩阵应该**不包含**畸变...
  • 摄像机矩阵由内参矩阵和外参矩阵组成,对摄像机矩阵进行QR分解可以得到内参矩阵和...外参包括旋转矩阵R3×3、平移向量T3×1,它们共同描述了如何把点从世界坐标系转换到摄像机坐标系,旋转矩阵描述了世界坐标系的坐...

    摄像机矩阵由内参矩阵和外参矩阵组成,对摄像机矩阵进行QR分解可以得到内参矩阵和外参矩阵。

    内参包括焦距、主点、倾斜系数、畸变系数

    (1)

    其中,fx,fy为焦距,一般情况下,二者相等,x0、y0为主点坐标(相对于成像平面),s为坐标轴倾斜参数,理想情况下为0

     

    外参包括旋转矩阵R3×3、平移向量T3×1,它们共同描述了如何把点从世界坐标系转换到摄像机坐标系,旋转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向,平移向量描述了在摄像机坐标系下空间原点的位置。

     

    分两种

    (1)世界坐标系——>像面坐标系

    首先将世界坐标系——>摄像机坐标系

    已知某点在世界坐标系中的坐标为(Xw, Yw, Zw),由旋转和平移矩阵可得摄像机坐标系和世界坐标系的关系为

    (2)

    然后将摄像机坐标系——>像面坐标系

     

    (3)

    其中[u v 1]T为点在图像坐标系中的坐标,[Xc Yc  Zc  1]T为点在摄像机坐标系中的坐标,K为摄像机内参数矩阵。

    这样最终可以得到:

    (4)

    (2)像面坐标系——>世界坐标系

    光轴会聚模型:

    对于两相机分别有:

    (5)          (6)

    公式56,左边Z应分别为Zc1,Zc2

    其中,

    (7)

    这样可以把(5)(6)写成

    (8)  

    公式8左边Z应为Zc1

    (9)

    公式9左边Z应为Zc2

    将(8)(9)整理可以得到

    (10)

    以上公式参考评论6楼 wisemanjack 

    采用最小二乘法求解X,Y,Z,在opencv中可以用solve(A,B,XYZ,DECOMP_SVD)求解

     

    代码如下:

     

    1. //opencv2.4.9 vs2012  
    2. #include <opencv2\opencv.hpp>  
    3. #include <fstream>  
    4. #include <iostream>  
    5.   
    6. using namespace std;  
    7. using namespace cv;  
    8.   
    9. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);  
    10. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);  
    11.   
    12. //图片对数量  
    13. int PicNum = 14;  
    14.   
    15. //左相机内参数矩阵  
    16. float leftIntrinsic[3][3] = {4037.82450,             0,     947.65449,  
    17.                                       0,    3969.79038,     455.48718,  
    18.                                       0,             0,             1};  
    19. //左相机畸变系数  
    20. float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};  
    21. //左相机旋转矩阵  
    22. float leftRotation[3][3] = {0.912333,       -0.211508,       0.350590,   
    23.                             0.023249,       -0.828105,      -0.560091,   
    24.                             0.408789,        0.519140,      -0.750590};  
    25. //左相机平移向量  
    26. float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};  
    27.   
    28. //右相机内参数矩阵  
    29. float rightIntrinsic[3][3] = {3765.83307,            0,     339.31958,  
    30.                                         0,  3808.08469,     660.05543,  
    31.                                         0,           0,             1};  
    32. //右相机畸变系数  
    33. float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};  
    34. //右相机旋转矩阵  
    35. float rightRotation[3][3] = {-0.134947,      0.989568,      -0.050442,   
    36.                               0.752355,      0.069205,      -0.655113,   
    37.                              -0.644788,     -0.126356,      -0.753845};  
    38. //右相机平移向量  
    39. float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};  
    40.   
    41.   
    42. int main()  
    43. {  
    44.     //已知空间坐标求成像坐标  
    45.     Point3f point(700,220,530);  
    46.     cout<<"左相机中坐标:"<<endl;  
    47.     cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;  
    48.     cout<<"右相机中坐标:"<<endl;  
    49.     cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;  
    50.   
    51.     //已知左右相机成像坐标求空间坐标  
    52.     Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);  
    53.     Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);  
    54.     Point3f worldPoint;  
    55.     worldPoint = uv2xyz(l,r);  
    56.     cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;  
    57.   
    58.     system("pause");  
    59.   
    60.     return 0;  
    61. }  
    62.   
    63.   
    64. //************************************  
    65. // Description: 根据左右相机中成像坐标求解空间坐标  
    66. // Method:    uv2xyz  
    67. // FullName:  uv2xyz  
    68. // Access:    public   
    69. // Parameter: Point2f uvLeft  
    70. // Parameter: Point2f uvRight  
    71. // Returns:   cv::Point3f  
    72. // Author:    小白  
    73. // Date:      2017/01/10  
    74. // History:  
    75. //************************************  
    76. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)  
    77. {  
    78.     //  [u1]      |X|                     [u2]      |X|  
    79.     //Z*|v1| = Ml*|Y|                   Z*|v2| = Mr*|Y|  
    80.     //  [ 1]      |Z|                     [ 1]      |Z|  
    81.     //            |1|                               |1|  
    82.     Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);  
    83.     Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);  
    84.     Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵  
    85.     hconcat(mLeftRotation,mLeftTranslation,mLeftRT);  
    86.     Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);  
    87.     Mat mLeftM = mLeftIntrinsic * mLeftRT;  
    88.     //cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;  
    89.   
    90.     Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);  
    91.     Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);  
    92.     Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵  
    93.     hconcat(mRightRotation,mRightTranslation,mRightRT);  
    94.     Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);  
    95.     Mat mRightM = mRightIntrinsic * mRightRT;  
    96.     //cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;  
    97.   
    98.     //最小二乘法A矩阵  
    99.     Mat A = Mat(4,3,CV_32F);  
    100.     A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);  
    101.     A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);  
    102.     A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);  
    103.   
    104.     A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);  
    105.     A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);  
    106.     A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);  
    107.   
    108.     A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);  
    109.     A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);  
    110.     A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);  
    111.   
    112.     A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);  
    113.     A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);  
    114.     A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);  
    115.   
    116.     //最小二乘法B矩阵  
    117.     Mat B = Mat(4,1,CV_32F);  
    118.     B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);  
    119.     B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);  
    120.     B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);  
    121.     B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);  
    122.   
    123.     Mat XYZ = Mat(3,1,CV_32F);  
    124.     //采用SVD最小二乘法求解XYZ  
    125.     solve(A,B,XYZ,DECOMP_SVD);  
    126.   
    127.     //cout<<"空间坐标为 = "<<endl<<XYZ<<endl;  
    128.   
    129.     //世界坐标系中坐标  
    130.     Point3f world;  
    131.     world.x = XYZ.at<float>(0,0);  
    132.     world.y = XYZ.at<float>(1,0);  
    133.     world.z = XYZ.at<float>(2,0);  
    134.   
    135.     return world;  
    136. }  
    137.   
    138. //************************************  
    139. // Description: 将世界坐标系中的点投影到左右相机成像坐标系中  
    140. // Method:    xyz2uv  
    141. // FullName:  xyz2uv  
    142. // Access:    public   
    143. // Parameter: Point3f worldPoint  
    144. // Parameter: float intrinsic[3][3]  
    145. // Parameter: float translation[1][3]  
    146. // Parameter: float rotation[3][3]  
    147. // Returns:   cv::Point2f  
    148. // Author:    小白  
    149. // Date:      2017/01/10  
    150. // History:  
    151. //************************************  
    152. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])  
    153. {  
    154.     //    [fx s x0]                         [Xc]        [Xw]        [u]   1     [Xc]  
    155.     //K = |0 fy y0|       TEMP = [R T]      |Yc| = TEMP*|Yw|        | | = —*K *|Yc|  
    156.     //    [ 0 0 1 ]                         [Zc]        |Zw|        [v]   Zc    [Zc]  
    157.     //                                                  [1 ]  
    158.     Point3f c;  
    159.     c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;  
    160.     c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;  
    161.     c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;  
    162.   
    163.     Point2f uv;  
    164.     uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z;  
    165.     uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z;  
    166.   
    167.     return uv;  
    168. }  


    matlab或者opencv标定完都是在左相机上建立世界坐标系,于是上面代码对应的改为:

     

    1. //左相机旋转矩阵    
    2. float leftRotation[3][3] = {1,0,0,  0,1,0,  0,0,1 };  
    3. //左相机平移向量    
    4. float leftTranslation[1][3] = {0,0,0};  

    畸变矩阵(默认获得5个即便参数k1,k2,p1,p2,k3)

     

     —————————————————————————————————————————————————

     像面坐标系——>世界坐标系还有一种模型是光轴平行模型

    双目立体视觉三位测量是基于视差原理:

    (11)

    (12)

    这里,除cx‘外的所有参数都来自于左图像,cx’是主点在右图像上的x坐标。如果主光线在无穷远处相交,那么cx=cx‘,并且右下角的项为0,给定一个二维齐次点和其关联的视差d,我们可以将此点投影到三维中:

    (13)

    三维坐标就是(X / W , Y / W , Z / W)

     

     

    1. Point p;  
    2. p.x =294,p.y=189;  
    3. cout<<p<< "in world coordinate: " << xyz.at<Vec3f>(p)*16 <<endl;   

    为什么要乘以16呢?

    因为在OpenCV2.0中,BM函数得出的结果是以16位符号数的形式的存储的,出于精度需要,所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下:

     

    1. dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);  

     

    可以看到,原始视差在左移8位(256)并且加上一个修正值之后又右移了4位,最终的结果就是左移4位

    因此,在实际求距离时,cvReprojectTo3D出来的X/W,Y/W,Z/W都要乘以16 (也就是W除以16),才能得到正确的三维坐标信息

    下面是在opencv3.0下实现的该方法的测距(转载)

     

     

    1. /******************************/  
    2. /*        立体匹配和测距        */  
    3. /******************************/  
    4.   
    5. #include <opencv2/opencv.hpp>    
    6. #include <iostream>    
    7.   
    8. using namespace std;  
    9. using namespace cv;  
    10.   
    11. const int imageWidth = 640;                             //摄像头的分辨率    
    12. const int imageHeight = 480;  
    13. Size imageSize = Size(imageWidth, imageHeight);  
    14.   
    15. Mat rgbImageL, grayImageL;  
    16. Mat rgbImageR, grayImageR;  
    17. Mat rectifyImageL, rectifyImageR;  
    18.   
    19. Rect validROIL;//图像校正之后,会对图像进行裁剪,这里的validROI就是指裁剪之后的区域    
    20. Rect validROIR;  
    21.   
    22. Mat mapLx, mapLy, mapRx, mapRy;     //映射表    
    23. Mat Rl, Rr, Pl, Pr, Q;              //校正旋转矩阵R,投影矩阵P 重投影矩阵Q  
    24. Mat xyz;              //三维坐标  
    25.   
    26. Point origin;         //鼠标按下的起始点  
    27. Rect selection;      //定义矩形选框  
    28. bool selectObject = false;    //是否选择对象  
    29.   
    30. int blockSize = 0, uniquenessRatio =0, numDisparities=0;  
    31. Ptr<StereoBM> bm = StereoBM::create(16, 9);  
    32.   
    33. /* 
    34. 事先标定好的相机的参数 
    35. fx 0 cx 
    36. 0 fy cy 
    37. 0 0  1 
    38. */  
    39. Mat cameraMatrixL = (Mat_<double>(3, 3) << 836.771593170594,0,319.970748854743,  
    40.     0,839.416501863912,228.788913693256,  
    41.     0, 0, 1);  
    42. Mat distCoeffL = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);  
    43.   
    44. Mat cameraMatrixR = (Mat_<double>(3, 3) << 838.101721655709,0,319.647150557935,  
    45.     0,840.636812165056,250.655818405938,  
    46.     0, 0, 1);  
    47. Mat distCoeffR = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);  
    48.   
    49. Mat T = (Mat_<double>(3, 1) << -39.7389449993974,0.0740619639178984,0.411914303245886);//T平移向量  
    50. Mat rec = (Mat_<double>(3, 1) << -0.00306, -0.03207, 0.00206);//rec旋转向量  
    51. Mat R = (Mat_<double>(3, 3) << 0.999957725513956,-0.00103511880221423,0.00913650447492805,  
    52.          0.00114462826834523,0.999927476064641,-0.0119888463633882,  
    53.          -0.00912343197938050,0.0119987974423658,0.999886389470751);//R 旋转矩阵  
    54.   
    55.   
    56. /*****立体匹配*****/  
    57. void stereo_match(int,void*)  
    58. {  
    59.     bm->setBlockSize(2*blockSize+5);     //SAD窗口大小,5~21之间为宜  
    60.     bm->setROI1(validROIL);  
    61.     bm->setROI2(validROIR);  
    62.     bm->setPreFilterCap(31);  
    63.     bm->setMinDisparity(0);  //最小视差,默认值为0, 可以是负值,int型  
    64.     bm->setNumDisparities(numDisparities*16+16);//视差窗口,即最大视差值与最小视差值之差,窗口大小必须是16的整数倍,int型  
    65.     bm->setTextureThreshold(10);   
    66.     bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配  
    67.     bm->setSpeckleWindowSize(100);  
    68.     bm->setSpeckleRange(32);  
    69.     bm->setDisp12MaxDiff(-1);  
    70.     Mat disp, disp8;  
    71.     bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图  
    72.     disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式  
    73.     reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时,ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16),才能得到正确的三维坐标信息。  
    74.     xyz = xyz * 16;  
    75.     imshow("disparity", disp8);  
    76. }  
    77.   
    78. /*****描述:鼠标操作回调*****/  
    79. static void onMouse(int event, int x, int y, int, void*)  
    80. {  
    81.     if (selectObject)  
    82.     {  
    83.         selection.x = MIN(x, origin.x);  
    84.         selection.y = MIN(y, origin.y);  
    85.         selection.width = std::abs(x - origin.x);  
    86.         selection.height = std::abs(y - origin.y);  
    87.     }  
    88.   
    89.     switch (event)  
    90.     {  
    91.     case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件  
    92.         origin = Point(x, y);  
    93.         selection = Rect(x, y, 0, 0);  
    94.         selectObject = true;  
    95.         cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;  
    96.         break;  
    97.     case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件  
    98.         selectObject = false;  
    99.         if (selection.width > 0 && selection.height > 0)  
    100.         break;  
    101.     }  
    102. }  
    103.   
    104.   
    105. /*****主函数*****/  
    106. int main()  
    107. {  
    108.     /* 
    109.     立体校正 
    110.     */  
    111.     //Rodrigues(rec, R); //Rodrigues变换  
    112.     stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,  
    113.         0, imageSize, &validROIL, &validROIR);  
    114.     initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);  
    115.     initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);  
    116.   
    117.     /* 
    118.     读取图片 
    119.     */  
    120.     rgbImageL = imread("左2.jpg", CV_LOAD_IMAGE_COLOR);  
    121.     cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);  
    122.     rgbImageR = imread("右2.jpg", CV_LOAD_IMAGE_COLOR);  
    123.     cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);  
    124.   
    125.     imshow("ImageL Before Rectify", grayImageL);  
    126.     imshow("ImageR Before Rectify", grayImageR);  
    127.   
    128.     /* 
    129.     经过remap之后,左右相机的图像已经共面并且行对准了 
    130.     */  
    131.     remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);  
    132.     remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);  
    133.   
    134.     /* 
    135.     把校正结果显示出来 
    136.     */  
    137.     Mat rgbRectifyImageL, rgbRectifyImageR;  
    138.     cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);  //伪彩色图  
    139.     cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);  
    140.   
    141.     //单独显示  
    142.     //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8);  
    143.     //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8);  
    144.     imshow("ImageL After Rectify", rgbRectifyImageL);  
    145.     imshow("ImageR After Rectify", rgbRectifyImageR);  
    146.   
    147.     //显示在同一张图上  
    148.     Mat canvas;  
    149.     double sf;  
    150.     int w, h;  
    151.     sf = 600. / MAX(imageSize.width, imageSize.height);  
    152.     w = cvRound(imageSize.width * sf);  
    153.     h = cvRound(imageSize.height * sf);  
    154.     canvas.create(h, w * 2, CV_8UC3);   //注意通道  
    155.   
    156.     //左图像画到画布上  
    157.     Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分    
    158.     resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小    
    159.     Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域      
    160.         cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));  
    161.     //rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形    
    162.     cout << "Painted ImageL" << endl;  
    163.   
    164.     //右图像画到画布上  
    165.     canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分    
    166.     resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);  
    167.     Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),  
    168.         cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));  
    169.     //rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);  
    170.     cout << "Painted ImageR" << endl;  
    171.   
    172.     //画上对应的线条  
    173.     for (int i = 0; i < canvas.rows; i += 16)  
    174.         line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);  
    175.     imshow("rectified", canvas);  
    176.   
    177.     /* 
    178.     立体匹配 
    179.     */  
    180.     namedWindow("disparity", CV_WINDOW_AUTOSIZE);  
    181.     // 创建SAD窗口 Trackbar  
    182.     createTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match);  
    183.     // 创建视差唯一性百分比窗口 Trackbar  
    184.     createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);  
    185.     // 创建视差窗口 Trackbar  
    186.     createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);  
    187.     //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数,一般取0)  
    188.     setMouseCallback("disparity", onMouse, 0);  
    189.     stereo_match(0,0);  
    190.   
    191.     waitKey(0);  
    192.     return 0;  
    193. }  


     

     

     

    ————————————————————————————————————————

     

     

     

    1. //opencv2.4.9 vs2012  
    2. #include <opencv2\opencv.hpp>  
    3. #include <fstream>  
    4. #include <iostream>  
    5.   
    6. using namespace std;  
    7. using namespace cv;  
    8.   
    9. //图片对数量  
    10. #define  PicNum  14  
    11.   
    12. //左相机内参数矩阵  
    13. float leftIntrinsic[3][3] = {4037.82450,             0,     947.65449,  
    14.                                       0,    3969.79038,     455.48718,  
    15.                                       0,             0,             1};  
    16. //左相机畸变系数  
    17. float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};  
    18. //左相机旋转矩阵  
    19. float leftRotation[3][3] = {0.912333,       -0.211508,       0.350590,   
    20.                             0.023249,       -0.828105,      -0.560091,   
    21.                             0.408789,        0.519140,      -0.750590};  
    22. //左相机平移向量  
    23. float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};  
    24.   
    25. //右相机内参数矩阵  
    26. float rightIntrinsic[3][3] = {3765.83307,            0,     339.31958,  
    27.                                         0,  3808.08469,     660.05543,  
    28.                                         0,           0,             1};  
    29. //右相机畸变系数  
    30. float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};  
    31. //右相机旋转矩阵  
    32. float rightRotation[3][3] = {-0.134947,      0.989568,      -0.050442,   
    33.                               0.752355,      0.069205,      -0.655113,   
    34.                              -0.644788,     -0.126356,      -0.753845};  
    35. //右相机平移向量  
    36. float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};  
    37.   
    38. //球坐标数组  
    39. //大球  
    40. float rightDaqiu[PicNum][2] = {0};  
    41. float leftDaqiu[PicNum][2] = {0};  
    42. float worldDaqiu[PicNum][3] = {0};  
    43. //小球  
    44. float rightXiaoqiu[PicNum][2] = {0};  
    45. float leftXiaoqiu[PicNum][2] = {0};  
    46. float worldXiaoqiu[PicNum][3] = {0};  
    47. //花球  
    48. float rightHuaqiu[PicNum][2] = {0};  
    49. float leftHuaqiu[PicNum][2] = {0};  
    50. float worldHuaqiu[PicNum][3] = {0};  
    51.   
    52. void ContrastAndBright(double alpha, double beta);//调节亮度/对比度  
    53. void CorrectionProcess();//对素材校正畸变  
    54. void initPos();//手动赋值球的图像坐标  
    55. void Daqiu();//计算大球图像坐标  
    56. Mat PictureCorrection( Mat image ,float intrinsic[3][3],float distortion[1][5]);//单张图像畸变校正  
    57. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);//从世界坐标转为图像坐标  
    58. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);//从图像坐标转为世界坐标  
    59.   
    60. int main()  
    61. {  
    62.     CorrectionProcess();//对素材校正畸变  
    63.     ContrastAndBright(2.5,50);//调节亮度/对比度  
    64.       
    65.     Daqiu();//自动计算大球坐标  
    66.       
    67.     initPos();//手动修正,如需验证数据,可以在该函数中修改  
    68.   
    69.     //求取大球的空间坐标  
    70.     cout<<"求解大球的世界坐标:"<<endl;  
    71.     for (int i=0; i<PicNum; i++)  
    72.     {  
    73.         Point2f l,r;  
    74.         Point3f worldPoint;  
    75.   
    76.         l.x = leftDaqiu[i][0];  
    77.         l.y = leftDaqiu[i][1];  
    78.         r.x = rightDaqiu[i][0];  
    79.         r.y = rightDaqiu[i][1];  
    80.   
    81.         worldPoint = uv2xyz(l,r);  
    82.         cout<< worldPoint <<endl;  
    83.         worldDaqiu[i][0] = worldPoint.x;  
    84.         worldDaqiu[i][1] = worldPoint.y;  
    85.         worldDaqiu[i][2] = worldPoint.z;  
    86.     }  
    87.   
    88.     cout<<"求解小球的世界坐标:"<<endl;  
    89.     for (int i=0; i<PicNum; i++)  
    90.     {  
    91.         Point2f l,r;  
    92.         Point3f worldPoint;  
    93.   
    94.         l.x = leftXiaoqiu[i][0];  
    95.         l.y = leftXiaoqiu[i][1];  
    96.         r.x = rightXiaoqiu[i][0];  
    97.         r.y = rightXiaoqiu[i][1];  
    98.   
    99.         worldPoint = uv2xyz(l,r);  
    100.         cout<< worldPoint <<endl;  
    101.         worldXiaoqiu[i][0] = worldPoint.x;  
    102.         worldXiaoqiu[i][1] = worldPoint.y;  
    103.         worldXiaoqiu[i][2] = worldPoint.z;  
    104.     }  
    105.   
    106.     cout<<"求解花球的世界坐标:"<<endl;  
    107.     for (int i=0; i<PicNum; i++)  
    108.     {  
    109.         Point2f l,r;  
    110.         Point3f worldPoint;  
    111.   
    112.         l.x = leftHuaqiu[i][0];  
    113.         l.y = leftHuaqiu[i][1];  
    114.         r.x = rightHuaqiu[i][0];  
    115.         r.y = rightHuaqiu[i][1];  
    116.   
    117.         worldPoint = uv2xyz(l,r);  
    118.         cout<< worldPoint <<endl;  
    119.         worldHuaqiu[i][0] = worldPoint.x;  
    120.         worldHuaqiu[i][1] = worldPoint.y;  
    121.         worldHuaqiu[i][2] = worldPoint.z;  
    122.     }  
    123.   
    124.     //csv文件写入部分  
    125.     ofstream oFile;  //定义文件输出流     
    126.   
    127.     oFile.open("三维坐标.csv", ios::out | ios::trunc);    //打开要输出的文件     这样就很容易的输出一个需要的excel 文件    
    128.     //写入大球数据  
    129.     oFile << "大球坐标" << endl;  
    130.     oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;  
    131.     oFile << "x,y,,x,y,,x,y,z" << endl;  
    132.     for (int i=0; i<PicNum ;i++)  
    133.     {  
    134.         oFile << leftDaqiu[i][0] <<","<< leftDaqiu[i][1] << ",," << rightDaqiu[i][0] <<","<< rightDaqiu[i][1]   
    135.               << ",," << worldDaqiu[i][0] <<","<<  worldDaqiu[i][1] <<","<<  worldDaqiu[i][2] << endl;  
    136.     }  
    137.   
    138.     //写入小球数据  
    139.     oFile << "小球坐标" << endl;  
    140.     oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;  
    141.     oFile << "x,y,,x,y,,x,y,z" << endl;  
    142.     for (int i=0; i<PicNum ;i++)  
    143.     {  
    144.         oFile << leftXiaoqiu[i][0] <<","<< leftXiaoqiu[i][1] << ",," << rightXiaoqiu[i][0] <<","<< rightXiaoqiu[i][1]   
    145.         << ",," << worldXiaoqiu[i][0] <<","<<  worldXiaoqiu[i][1] <<","<<  worldXiaoqiu[i][2] << endl;  
    146.     }  
    147.       
    148.   
    149.     //写入花球数据  
    150.     oFile << "花球坐标" << endl;  
    151.     oFile << "左相机坐标,,,右相机坐标,,,世界坐标" << endl;  
    152.     oFile << "x,y,,x,y,,x,y,z" << endl;  
    153.     for (int i=0; i<PicNum ;i++)  
    154.     {  
    155.         oFile << leftHuaqiu[i][0] <<","<< leftHuaqiu[i][1] << ",," << rightHuaqiu[i][0] <<","<< rightHuaqiu[i][1]   
    156.         << ",," << worldHuaqiu[i][0] <<","<<  worldHuaqiu[i][1] <<","<<  worldHuaqiu[i][2] << endl;  
    157.     }  
    158.   
    159.     //关闭文件  
    160.     oFile.close();    
    161.       
    162.   
    163.     //test  
    164.     已知空间坐标求成像坐标  
    165.     //Point3f point(700,220,530);  
    166.     //cout<<"左相机中坐标:"<<endl;  
    167.     //cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;  
    168.     //cout<<"右相机中坐标:"<<endl;  
    169.     //cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;  
    170.   
    171.     //已知左右相机成像坐标求空间坐标  
    172.     //Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);  
    173.     //Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);  
    174.     //Point3f worldPoint;  
    175.     //worldPoint = uv2xyz(l,r);  
    176.     //cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;  
    177.   
    178.   
    179.     /* 
    180.     //csv文件读取部分 
    181.  
    182.     string value;//临时字符串 
    183.     ifstream iFile("三维坐标.csv");//打开要读入的文件 
    184.     //循环行读取 
    185.     while (iFile.good()) 
    186.     { 
    187.         getline(iFile,value); 
    188.         //getline(iFile,value,','); //.csv文件用","作为分隔符 
    189.         cout<<value<<endl; 
    190.     } 
    191.     */  
    192.       
    193.     system("pause");  
    194.   
    195.     return 0;  
    196. }  
    197.   
    198. //************************************  
    199. // Description: 修正圆心坐标  
    200. // Method:    initPos  
    201. // FullName:  initPos  
    202. // Access:    public   
    203. // Returns:   void  
    204. // Author:    bhy  
    205. // Date:      2016/12/25  
    206. // History:  
    207. //************************************  
    208. void initPos()  
    209. {  
    210.     //手动修正  
    211.     leftDaqiu[0][0] = 1175;  leftDaqiu[0][1] = 7;     rightDaqiu[0][0] = 256;  rightDaqiu[0][1] = 1;  
    212.     leftDaqiu[1][0] = 823;   leftDaqiu[1][1] = 603;   rightDaqiu[1][0] = 289;  rightDaqiu[1][1] = 431;  
    213.     leftDaqiu[2][0] = 963;   leftDaqiu[2][1] = 360;   rightDaqiu[2][0] = 283;  rightDaqiu[2][1] = 169;  
    214.     leftDaqiu[3][0] = 1065;  leftDaqiu[3][1] = 180;   rightDaqiu[3][0] = 294;  rightDaqiu[3][1] = 1;  
    215.     leftDaqiu[4][0] = 1039;  leftDaqiu[4][1] = 217;   rightDaqiu[4][0] = 314;  rightDaqiu[4][1] = 68;  
    216.     leftDaqiu[5][0] = 896;   leftDaqiu[5][1] = 448;   rightDaqiu[5][0] = 378;  rightDaqiu[5][1] = 402;  
    217.     leftDaqiu[6][0] = 933;   leftDaqiu[6][1] = 376;   rightDaqiu[6][0] = 398;  rightDaqiu[6][1] = 347;  
    218.     leftDaqiu[7][0] = 868;   leftDaqiu[7][1] = 463;   rightDaqiu[7][0] = 423;  rightDaqiu[7][1] = 418;  
    219.     leftDaqiu[8][0] = 878;   leftDaqiu[8][1] = 417;   rightDaqiu[8][0] = 458;  rightDaqiu[8][1] = 466;  
    220.     leftDaqiu[9][0] = 860;   leftDaqiu[9][1] = 423;   rightDaqiu[9][0] = 481;  rightDaqiu[9][1] = 490;  
    221.     leftDaqiu[10][0] = 840;  leftDaqiu[10][1] = 442;  rightDaqiu[10][0] = 499; rightDaqiu[10][1] = 500;  
    222.     leftDaqiu[11][0] = 822;  leftDaqiu[11][1] = 414;  rightDaqiu[11][0] = 523; rightDaqiu[11][1] = 511;  
    223.     leftDaqiu[12][0] = 805;  leftDaqiu[12][1] = 406;  rightDaqiu[12][0] = 538; rightDaqiu[12][1] = 516;  
    224.     leftDaqiu[13][0] = 802;  leftDaqiu[13][1] = 402;  rightDaqiu[13][0] = 549; rightDaqiu[13][1] = 514;  
    225.   
    226.     leftXiaoqiu[0][0] = 1250;   leftXiaoqiu[0][1] = 120;   rightXiaoqiu[0][0] = 308;  rightXiaoqiu[0][1] = 313;  
    227.     leftXiaoqiu[1][0] = 1034;   leftXiaoqiu[1][1] = 481;   rightXiaoqiu[1][0] = 314;  rightXiaoqiu[1][1] = 482;  
    228.     leftXiaoqiu[2][0] = 1207;   leftXiaoqiu[2][1] = 228;   rightXiaoqiu[2][0] = 284;  rightXiaoqiu[2][1] = 186;  
    229.     leftXiaoqiu[3][0] = 1343;   leftXiaoqiu[3][1] = 55;    rightXiaoqiu[3][0] = 252;  rightXiaoqiu[3][1] = -20;  
    230.     leftXiaoqiu[4][0] = 1326;   leftXiaoqiu[4][1] = 102;   rightXiaoqiu[4][0] = 242;  rightXiaoqiu[4][1] = 23;  
    231.     leftXiaoqiu[5][0] = 1021;   leftXiaoqiu[5][1] = 625;   rightXiaoqiu[5][0] = 269;  rightXiaoqiu[5][1] = 632;  
    232.     leftXiaoqiu[6][0] = 1123;   leftXiaoqiu[6][1] = 489;   rightXiaoqiu[6][0] = 241;  rightXiaoqiu[6][1] = 458;  
    233.     leftXiaoqiu[7][0] = 1147;   leftXiaoqiu[7][1] = 475;   rightXiaoqiu[7][0] = 224;  rightXiaoqiu[7][1] = 404;  
    234.     leftXiaoqiu[8][0] = 1078;   leftXiaoqiu[8][1] = 595;   rightXiaoqiu[8][0] = 223;  rightXiaoqiu[8][1] = 558;  
    235.     leftXiaoqiu[9][0] = 1062;   leftXiaoqiu[9][1] = 635;   rightXiaoqiu[9][0] = 216;  rightXiaoqiu[9][1] = 598;  
    236.     leftXiaoqiu[10][0] = 1080;  leftXiaoqiu[10][1] = 619;  rightXiaoqiu[10][0] = 201; rightXiaoqiu[10][1] = 576;  
    237.     leftXiaoqiu[11][0] = 1054;  leftXiaoqiu[11][1] = 690;  rightXiaoqiu[11][0] = 189; rightXiaoqiu[11][1] = 633;  
    238.     leftXiaoqiu[12][0] = 1046;  leftXiaoqiu[12][1] = 724;  rightXiaoqiu[12][0] = 179; rightXiaoqiu[12][1] = 655;  
    239.     leftXiaoqiu[13][0] = 1046;  leftXiaoqiu[13][1] = 726;  rightXiaoqiu[13][0] = 172; rightXiaoqiu[13][1] = 656;  
    240.       
    241.     leftHuaqiu[0][0] = 1075;  leftHuaqiu[0][1] = 111;   rightHuaqiu[0][0] = 120;  rightHuaqiu[0][1] = -30;//出视场  
    242.     leftHuaqiu[1][0] = 708;   leftHuaqiu[1][1] = 810;   rightHuaqiu[1][0] = 142;  rightHuaqiu[1][1] = 382;  
    243.     leftHuaqiu[2][0] = 876;   leftHuaqiu[2][1] = 518;   rightHuaqiu[2][0] = 131;  rightHuaqiu[2][1] = 83;  
    244.     leftHuaqiu[3][0] = 1021;  leftHuaqiu[3][1] = 253;   rightHuaqiu[3][0] = 180;   rightHuaqiu[3][1] = -60;//出视场  
    245.     leftHuaqiu[4][0] = 1019;  leftHuaqiu[4][1] = 248;   rightHuaqiu[4][0] = 183;   rightHuaqiu[4][1] = -30;//出视场  
    246.     leftHuaqiu[5][0] = 764;   leftHuaqiu[5][1] = 654;   rightHuaqiu[5][0] = 278;  rightHuaqiu[5][1] = 437;  
    247.     leftHuaqiu[6][0] = 844;   leftHuaqiu[6][1] = 486;   rightHuaqiu[6][0] = 286;  rightHuaqiu[6][1] = 261;  
    248.     leftHuaqiu[7][0] = 852;   leftHuaqiu[7][1] = 425;   rightHuaqiu[7][0] = 305;  rightHuaqiu[7][1] = 206;  
    249.     leftHuaqiu[8][0] = 780;   leftHuaqiu[8][1] = 523;   rightHuaqiu[8][0] = 347;  rightHuaqiu[8][1] = 359;  
    250.     leftHuaqiu[9][0] = 757;   leftHuaqiu[9][1] = 530;   rightHuaqiu[9][0] = 363;  rightHuaqiu[9][1] = 382;  
    251.     leftHuaqiu[10][0] = 765;  leftHuaqiu[10][1] = 505;  rightHuaqiu[10][0] = 368; rightHuaqiu[10][1] = 345;  
    252.     leftHuaqiu[11][0] = 702;  leftHuaqiu[11][1] = 554;  rightHuaqiu[11][0] = 382; rightHuaqiu[11][1] = 398;  
    253.     leftHuaqiu[12][0] = 666;  leftHuaqiu[12][1] = 573;  rightHuaqiu[12][0] = 386; rightHuaqiu[12][1] = 408;  
    254.     leftHuaqiu[13][0] = 656;  leftHuaqiu[13][1] = 581;  rightHuaqiu[13][0] = 384; rightHuaqiu[13][1] = 398;  
    255. }  
    256.   
    257. //************************************  
    258. // 2016/12/2  by 小白  
    259. // Method:    uv2xyz  
    260. // FullName:  uv2xyz  
    261. // Access:    public   
    262. // Returns:   cv::Point3f       世界坐标  
    263. // Qualifier: 根据左右相机中成像坐标求解空间坐标  
    264. // Parameter: Point2f uvLeft        左相机中成像坐标  
    265. // Parameter: Point2f uvRight       右相机中成像坐标  
    266. //************************************  
    267. Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)  
    268. {  
    269.     //  [u1]      |X|                     [u2]      |X|  
    270.     //Z*|v1| = Ml*|Y|                   Z*|v2| = Mr*|Y|  
    271.     //  [ 1]      |Z|                     [ 1]      |Z|  
    272.     //            |1|                               |1|  
    273.     Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);  
    274.     Mat mLeftTranslation = Mat(3,1,CV_32F,leftTranslation);  
    275.     Mat mLeftRT = Mat(3,4,CV_32F);//左相机M矩阵  
    276.     hconcat(mLeftRotation,mLeftTranslation,mLeftRT);  
    277.     Mat mLeftIntrinsic = Mat(3,3,CV_32F,leftIntrinsic);  
    278.     Mat mLeftM = mLeftIntrinsic * mLeftRT;  
    279.     //cout<<"左相机M矩阵 = "<<endl<<mLeftM<<endl;  
    280.   
    281.     Mat mRightRotation = Mat(3,3,CV_32F,rightRotation);  
    282.     Mat mRightTranslation = Mat(3,1,CV_32F,rightTranslation);  
    283.     Mat mRightRT = Mat(3,4,CV_32F);//右相机M矩阵  
    284.     hconcat(mRightRotation,mRightTranslation,mRightRT);  
    285.     Mat mRightIntrinsic = Mat(3,3,CV_32F,rightIntrinsic);  
    286.     Mat mRightM = mRightIntrinsic * mRightRT;  
    287.     //cout<<"右相机M矩阵 = "<<endl<<mRightM<<endl;  
    288.   
    289.     //最小二乘法A矩阵  
    290.     Mat A = Mat(4,3,CV_32F);  
    291.     A.at<float>(0,0) = uvLeft.x * mLeftM.at<float>(2,0) - mLeftM.at<float>(0,0);  
    292.     A.at<float>(0,1) = uvLeft.x * mLeftM.at<float>(2,1) - mLeftM.at<float>(0,1);  
    293.     A.at<float>(0,2) = uvLeft.x * mLeftM.at<float>(2,2) - mLeftM.at<float>(0,2);  
    294.   
    295.     A.at<float>(1,0) = uvLeft.y * mLeftM.at<float>(2,0) - mLeftM.at<float>(1,0);  
    296.     A.at<float>(1,1) = uvLeft.y * mLeftM.at<float>(2,1) - mLeftM.at<float>(1,1);  
    297.     A.at<float>(1,2) = uvLeft.y * mLeftM.at<float>(2,2) - mLeftM.at<float>(1,2);  
    298.   
    299.     A.at<float>(2,0) = uvRight.x * mRightM.at<float>(2,0) - mRightM.at<float>(0,0);  
    300.     A.at<float>(2,1) = uvRight.x * mRightM.at<float>(2,1) - mRightM.at<float>(0,1);  
    301.     A.at<float>(2,2) = uvRight.x * mRightM.at<float>(2,2) - mRightM.at<float>(0,2);  
    302.   
    303.     A.at<float>(3,0) = uvRight.y * mRightM.at<float>(2,0) - mRightM.at<float>(1,0);  
    304.     A.at<float>(3,1) = uvRight.y * mRightM.at<float>(2,1) - mRightM.at<float>(1,1);  
    305.     A.at<float>(3,2) = uvRight.y * mRightM.at<float>(2,2) - mRightM.at<float>(1,2);  
    306.   
    307.     //最小二乘法B矩阵  
    308.     Mat B = Mat(4,1,CV_32F);  
    309.     B.at<float>(0,0) = mLeftM.at<float>(0,3) - uvLeft.x * mLeftM.at<float>(2,3);  
    310.     B.at<float>(1,0) = mLeftM.at<float>(1,3) - uvLeft.y * mLeftM.at<float>(2,3);  
    311.     B.at<float>(2,0) = mRightM.at<float>(0,3) - uvRight.x * mRightM.at<float>(2,3);  
    312.     B.at<float>(3,0) = mRightM.at<float>(1,3) - uvRight.y * mRightM.at<float>(2,3);  
    313.   
    314.     Mat XYZ = Mat(3,1,CV_32F);  
    315.     //采用SVD最小二乘法求解XYZ  
    316.     solve(A,B,XYZ,DECOMP_SVD);  
    317.   
    318.     //cout<<"空间坐标为 = "<<endl<<XYZ<<endl;  
    319.   
    320.     //世界坐标系中坐标  
    321.     Point3f world;  
    322.     world.x = XYZ.at<float>(0,0);  
    323.     world.y = XYZ.at<float>(1,0);  
    324.     world.z = XYZ.at<float>(2,0);  
    325.   
    326.     return world;  
    327. }  
    328.   
    329. //************************************  
    330. // Description: 将空间坐标转换为像面坐标,用于检验  
    331. // Method:    xyz2uv  
    332. // FullName:  xyz2uv  
    333. // Access:    public   
    334. // Parameter: Point3f worldPoint  
    335. // Parameter: float intrinsic[3][3]  
    336. // Parameter: float translation[1][3]  
    337. // Parameter: float rotation[3][3]  
    338. // Returns:   cv::Point2f  
    339. // Author:    bhy  
    340. // Date:      2016/12/28  
    341. // History:  
    342. //************************************  
    343. Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3])  
    344. {  
    345.     //    [fx s x0]                         [Xc]        [Xw]        [u]   1     [Xc]  
    346.     //K = |0 fy y0|       TEMP = [R T]      |Yc| = TEMP*|Yw|        | | = —*K *|Yc|  
    347.     //    [ 0 0 1 ]                         [Zc]        |Zw|        [v]   Zc    [Zc]  
    348.     //                                                  [1 ]  
    349.     Point3f c;  
    350.     c.x = rotation[0][0]*worldPoint.x + rotation[0][1]*worldPoint.y + rotation[0][2]*worldPoint.z + translation[0][0]*1;  
    351.     c.y = rotation[1][0]*worldPoint.x + rotation[1][1]*worldPoint.y + rotation[1][2]*worldPoint.z + translation[0][1]*1;  
    352.     c.z = rotation[2][0]*worldPoint.x + rotation[2][1]*worldPoint.y + rotation[2][2]*worldPoint.z + translation[0][2]*1;  
    353.   
    354.     Point2f uv;  
    355.     uv.x = (intrinsic[0][0]*c.x + intrinsic[0][1]*c.y + intrinsic[0][2]*c.z)/c.z + 0.5;//加0.5去整 == 四舍五入  
    356.     uv.y = (intrinsic[1][0]*c.x + intrinsic[1][1]*c.y + intrinsic[1][2]*c.z)/c.z + 0.5;//加0.5去整 == 四舍五入  
    357.   
    358.     return uv;  
    359. }  
    360.   
    361.   
    362. //************************************  
    363. // Description: 对畸变校正文件夹下的图片批量进行亮度/对比度调节  
    364. // Method:    ContrastAndBright  
    365. // FullName:  ContrastAndBright  
    366. // Access:    public   
    367. // Parameter: double alpha  
    368. // Parameter: double beta  
    369. // Returns:   void  
    370. // Author:    bhy  
    371. // Date:      2016/12/28  
    372. // History:  
    373. //************************************  
    374. void ContrastAndBright(double alpha, double beta)  
    375. {  
    376.     //double alpha =3;    
    377.     //double beta = 40;   
    378.     cout<<"**********************************************"<<endl;  
    379.     cout<<"                 亮度/对比度调节              "<<endl;  
    380.     cout<<"**********************************************"<<endl;  
    381.   
    382.     cout<<"当前 alpha = "<<alpha<<endl;  
    383.     cout<<"当前 beta = "<<beta<<endl;  
    384.   
    385.     Mat src,dst;   
    386.   
    387.     system("md 亮度对比度\\rightky1");  
    388.   
    389.     //右相机调节  
    390.     //如果校正图像目录不存在,则创建该目录  
    391.   
    392.     for (int ii=1; ii<=PicNum; ii++)  
    393.     {  
    394.         cout<<"右:第"<<ii<<"张图片"<<endl;  
    395.   
    396.         char* filename = new char[100];  
    397.         sprintf(filename,"畸变校正/rightky1/r%d.bmp",ii);  
    398.         src = imread(filename);//顺次读入图片  
    399.         delete []filename;//释放字符串  
    400.           
    401.         dst = Mat::zeros(src.size(),src.type());//清空目标矩阵  
    402.         //根据alpha,beta重新计算灰度值  
    403.         for (int i = 0;i<src.rows;++i)    
    404.             for(int j= 0;j<src.cols;++j)    
    405.                 for(int k = 0;k<3;++k)    
    406.                     dst.at<Vec3b>(i,j)[k] = saturate_cast<uchar>(src.at<Vec3b>(i,j)[k]*alpha+beta);    
    407.   
    408.         char* output = new char[100];  
    409.         sprintf(output,"亮度对比度/rightky1/r%d.bmp",ii);  
    410.         imwrite(output,dst); //顺次保存校正图  
    411.         delete []output;//释放字符串  
    412.     }  
    413.   
    414.     //左相机调节  
    415.     //如果校正图像目录不存在,则创建该目录  
    416.     system("md 亮度对比度\\leftky1");  
    417.   
    418.     for (int ii=1; ii<=PicNum; ii++)  
    419.     {  
    420.         cout<<"左:第"<<ii<<"张图片"<<endl;  
    421.   
    422.         char* filename = new char[100];  
    423.         sprintf(filename,"畸变校正/leftky1/l%d.bmp",ii);  
    424.         src = imread(filename);//顺次读入图片  
    425.         delete []filename;//释放字符串  
    426.   
    427.         dst = Mat::zeros(src.size(),src.type());//清空目标矩阵  
    428.         //根据alpha,beta重新计算灰度值  
    429.         for (int i = 0;i<src.rows;++i)    
    430.             for(int j= 0;j<src.cols;++j)    
    431.                 for(int k = 0;k<3;++k)    
    432.                     dst.at<Vec3b>(i,j)[k] = saturate_cast<uchar>(src.at<Vec3b>(i,j)[k]*alpha+beta);    
    433.   
    434.         char* output = new char[100];  
    435.         sprintf(output,"亮度对比度/leftky1/l%d.bmp",ii);  
    436.         imwrite(output,dst); //顺次保存校正图  
    437.         delete []output;//释放字符串  
    438.     }  
    439.   
    440.     cout<<"**********************************************"<<endl;  
    441.     cout<<"             亮度/对比度调节结束              "<<endl;  
    442.     cout<<"**********************************************"<<endl;  
    443. }  
    444.   
    445. //************************************  
    446. // Description: 对素材文件夹中的图片批量进行畸变校正  
    447. // Method:    CorrectionProcess  
    448. // FullName:  CorrectionProcess  
    449. // Access:    public   
    450. // Returns:   void  
    451. // Author:    bhy  
    452. // Date:      2016/12/28  
    453. // History:  
    454. //************************************  
    455. void CorrectionProcess()  
    456. {  
    457.     cout<<"**********************************************"<<endl;  
    458.     cout<<"                    畸变校正                  "<<endl;  
    459.     cout<<"**********************************************"<<endl;  
    460.   
    461.     Mat image;  
    462.   
    463.     //使用畸变系数与内参校正右相机原图  
    464.     //如果校正图像目录不存在,则创建该目录  
    465.     system("md 畸变校正\\rightky1");  
    466.   
    467.     for (int i=1; i<=PicNum; i++)  
    468.     {  
    469.         cout<<"右:校正第"<<i<<"张图片"<<endl;  
    470.   
    471.         char* filename = new char[100];  
    472.         sprintf(filename,"素材/rightky1/r%d.bmp",i);  
    473.         image = imread(filename,IMREAD_GRAYSCALE);//顺次读入图片  
    474.         delete []filename;//释放字符串  
    475.   
    476.         char* output = new char[100];  
    477.         sprintf(output,"畸变校正/rightky1/r%d.bmp",i);  
    478.         imwrite(output,PictureCorrection(image,rightIntrinsic,rightDistortion)); //顺次保存校正图  
    479.         delete []output;//释放字符串  
    480.     }  
    481.   
    482.     //使用畸变系数与内参校正左相机原图  
    483.     //如果校正图像目录不存在,则创建该目录  
    484.     system("md 畸变校正\\leftky1");  
    485.   
    486.     for (int i=1; i<=PicNum; i++)  
    487.     {  
    488.         cout<<"左:校正第"<<i<<"张图片"<<endl;  
    489.   
    490.         char* filename = new char[100];  
    491.         sprintf(filename,"素材/leftky1/l%d.bmp",i);  
    492.         image = imread(filename,IMREAD_GRAYSCALE);//顺次读入图片  
    493.         delete []filename;//释放字符串  
    494.   
    495.         char* output = new char[100];  
    496.         sprintf(output,"畸变校正/leftky1/l%d.bmp",i);  
    497.         imwrite(output,PictureCorrection(image,leftIntrinsic,leftDistortion)); //顺次保存校正图  
    498.         delete []output;//释放字符串  
    499.     }  
    500.     cout<<"**********************************************"<<endl;  
    501.     cout<<"                  畸变校正结束                "<<endl;  
    502.     cout<<"**********************************************"<<endl;  
    503. }  
    504.   
    505. //************************************  
    506. // Method:    PictureCorrection  
    507. // FullName:  PictureCorrection  
    508. // Access:    public   
    509. // Returns:   cv::Mat   校正图  
    510. // Qualifier: 根据畸变系数与内参校正一张图片  
    511. // Parameter: Mat image  
    512. // Parameter: float intrinsic[3][3]     内参  
    513. // Parameter: float distortion[1][5]    畸变矩阵  
    514. //************************************  
    515. Mat PictureCorrection( Mat image ,float intrinsic[3][3],float distortion[1][5])  
    516. {  
    517.     Size image_size = image.size();  
    518.   
    519.     Mat intrinsic_matrix = Mat(3,3,CV_32FC1,intrinsic);  
    520.     Mat distortion_coeffs = Mat(1,5,CV_32FC1,distortion);  
    521.     Mat R = Mat::eye(3,3,CV_32F);         
    522.     Mat mapx = Mat(image_size,CV_32FC1);  
    523.     Mat mapy = Mat(image_size,CV_32FC1);      
    524.     initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,mapx,mapy);  
    525.     Mat t = image.clone();  
    526.     cv::remap( image, t, mapx, mapy, INTER_LINEAR);  
    527.     return t;  
    528. }  
    529.   
    530.   
    531. //************************************  
    532. // Description: 采用hough变换求取大球圆心  
    533. // Method:    Daqiu  
    534. // FullName:  Daqiu  
    535. // Access:    public   
    536. // Returns:   void  
    537. // Author:    bhy  
    538. // Date:      2016/12/28  
    539. // History:  
    540. //************************************  
    541. void Daqiu()  
    542. {  
    543.     cout<<"**********************************************"<<endl;  
    544.     cout<<"                  计算大球圆心                "<<endl;  
    545.     cout<<"**********************************************"<<endl;  
    546.   
    547.     Mat img;  
    548.     //右相机 ContrastAndBright(2.5,50);  
    549.     system("md 大球圆心\\rightky1");  
    550.   
    551.     cout<<"右图:"<<endl;  
    552.   
    553.     for (int i=1; i<=PicNum; i++)  
    554.     {  
    555.         stringstream strStm;  
    556.         string strFileName;  
    557.         strStm << i;  
    558.         strStm >> strFileName;  
    559.         strFileName = "亮度对比度/rightky1/r" + strFileName + ".bmp";  
    560.         img = imread(strFileName,IMREAD_GRAYSCALE);  
    561.   
    562.         GaussianBlur(img,img,Size(5,5),0);  
    563.   
    564.         vector<Vec3f> circles;  
    565.         HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 70, 30, 95 ,100);//hough圆变换  
    566.         Point2f center(0,0);  
    567.         float radius;  
    568.         for (int j = 0; j < circles.size(); j++)  
    569.         {  
    570.             if (circles[j][0] > center.x && circles[j][0] < img.cols/2)  
    571.             {  
    572.                 center.x = circles[j][0];  
    573.                 center.y = circles[j][1];  
    574.                 radius = circles[j][2];//半径   
    575.             }  
    576.         }  
    577.   
    578.         rightDaqiu[i-1][0] = center.x;  
    579.         rightDaqiu[i-1][1] = center.y;  
    580.   
    581.         CvScalar color = CV_RGB(0,0,0);  
    582.         circle( img, (Point)center, radius, color, 3, 8, 0);//绘制圆  
    583.         circle( img, (Point)center, 3, color, 3, 8, 0);//绘制圆心  
    584.         cout<<"圆心:"<<center<<endl;//为了保证精度,以原值输出  
    585.   
    586.         char* output = new char[100];  
    587.         sprintf(output,"大球圆心/rightky1/r%d.bmp",i);  
    588.         imwrite(output,img); //顺次保存校正图  
    589.         delete []output;//释放字符串  
    590.     }  
    591.   
    592.     system("md 大球圆心\\leftky1");  
    593.   
    594.     cout<<"左图:"<<endl;  
    595.   
    596.     for (int i=1; i<=PicNum; i++)  
    597.     {  
    598.         stringstream strStm;  
    599.         string strFileName;  
    600.         strStm << i;  
    601.         strStm >> strFileName;  
    602.         strFileName = "亮度对比度/leftky1/l" + strFileName + ".bmp";  
    603.         img = imread(strFileName,IMREAD_GRAYSCALE);  
    604.   
    605.         GaussianBlur(img,img,Size(5,5),0);  
    606.   
    607.         vector<Vec3f> circles;  
    608.         HoughCircles( img, circles, CV_HOUGH_GRADIENT, 3 ,70, 30, 50, 100 ,110);//hough圆变换  
    609.         Point2f center(0,1024);  
    610.         float radius;  
    611.         for (int j = 0; j < circles.size(); j++)  
    612.         {  
    613.             if (circles[j][1]<center.y)  
    614.             {  
    615.                 center.x = circles[j][0];  
    616.                 center.y = circles[j][1];  
    617.                 radius = circles[j][2];//半径  
    618.             }  
    619.         }  
    620.         leftDaqiu[i-1][0] = center.x;  
    621.         leftDaqiu[i-1][1] = center.y;  
    622.   
    623.         CvScalar color = CV_RGB(0,0,0);  
    624.         circle( img, center, radius, color, 3, 8, 0);//绘制圆  
    625.         circle( img, center, 3, color, 3, 8, 0);//绘制圆心  
    626.         cout<<"圆心:"<<center<<endl;//为了保证精度,以原值输出  
    627.   
    628.         char* output = new char[100];  
    629.         sprintf(output,"大球圆心/leftky1/l%d.bmp",i);  
    630.         imwrite(output,img); //顺次保存校正图  
    631.         delete []output;//释放字符串  
    632.     }  
    633.   
    634.     cout<<"**********************************************"<<endl;  
    635.     cout<<"                  圆心计算结束                "<<endl;  
    636.     cout<<"**********************************************"<<endl;  
    637. }  


     

    运行该程序,会提取生成的“三维坐标.csv”中的空间坐标数据,并绘制运动轨迹需要注意的是,在“三维坐标.csv”文件中直接修改圆心坐标没有用,需要在工程中的initPos()修改。

    [cpp] view plain copy

    1. clc;clear;  
    2. M = csvread('三维坐标.csv',3,6,[3,6,16,8]);  
    3. x = M(:,1);  
    4. y = M(:,2);  
    5. z = M(:,3);  
    6. plot3(x,y,z,'r');  
    7. %legend('大球');  
    8. hold on  
    9.   
    10. M = csvread('三维坐标.csv',20,6,[20,6,33,8]);  
    11. x = M(:,1);  
    12. y = M(:,2);  
    13. z = M(:,3);  
    14. plot3(x,y,z,'g');  
    15. %legend('小球');  
    16. hold on  
    17.   
    18. M = csvread('三维坐标.csv',37,6,[37,6,50,8]);  
    19. x = M(:,1);  
    20. y = M(:,2);  
    21. z = M(:,3);  
    22. plot3(x,y,z,'b');  
    23. %legend('花球');  
    24. hold on  
    25.   
    26. legend('大球','小球','花球');  
    27. title('小球运动轨迹');  
    28. xlabel('x');  
    29. ylabel('y');  
    30. zlabel('z');  
    31. grid on  
    32. axis square  


    最后绘制的轨迹图

     

     补一张世界坐标系的图

    展开全文
  • 两个坐标系之间变换矩阵的实现

    万次阅读 2014-12-04 10:15:15
    主要分析从局部坐标系变换到全局坐标系下的坐标转换公式。首先对全局坐标系进行描述,如图1,o-xyz即全局坐标系,O’-X’Y’Z’即在o-xyz坐标系中定义的局部坐标系,两个坐标系之间的关系如下:这个O’-X’Y’Z’的Z...
  • 一个平面坐标系逆时针旋转一个角度后得到另一个坐标系,则同一个点在这两个坐标系之间的几何关系如下: 由上图可得: 转化为矩阵表示为: 则反过来的关系如下: 由上面两个矩阵式可以看出两个转换的旋转矩阵是...
  • 工作中,因为涉及到坐标系变换。所以使用到矩阵。但因为数学基础不佳,矩阵变换始终困扰着我。 此处,作为记录。 PS:矩阵具体介绍详情百度。...使用此矩阵可以进行平面坐标系的旋转,缩放和方向变换。 { a,
  • 计算机视觉-相机内参数和外参数-相机标定1、 相机内参2、相机外参数3、相机标定(或摄像机标定):4、坐标转换 1、 相机内参 相机内参数是与相机自身特性相关的参数,比如相机的焦距、像素大小等; 相机内参分为内参...
  • 打算通过像素坐标得到对应物体在世界坐标系的三维坐标,但是得到旋转矩阵和平移向量不知道怎么用,内外参乘起来之后,用三维坐标去还原二维坐标,得到的跟实际二维坐标相差十万八千里。 所以最后打算不求内参和外参...
  • VTK:坐标转换

    2021-04-24 16:21:28
    为了包括诸如消失点之类的投影效果,我们使用一种称为齐次坐标的特殊坐标系。 使用齐次坐标,我们可以通过将wh设置为零来表示一个无限点。相机使用此功能进行透视转换。通过使用4x4转换矩阵来应用转换。变换矩阵...
  • 计算机视觉当中相机成像模型坐标转换世界坐标系到相机坐标系转换  世界坐标系是在环境当中选定的一个三维坐标系,用于描述环境中任何物体的位置,符合右手坐标系。相机坐标系的原点位于镜头的光心,x,y轴分别与...
  • 相机标定简单来说是从世界坐标系转换到图像坐标系的过程,也就是求最终的投影矩阵的过程。 基本的坐标系:世界坐标系、相机坐标系、成像平面坐标系、像素坐标系。 一般来说,标定的过程分为两个部分: 1、从...
  • 要想把点云从全局坐标系下转化到XoY的二维平面,只需把点按照旋转矩阵R变换后,PCA返回的三个向量就组成了3X3的旋转矩阵R。 换句话说,PCA对平面点云的变换,就是把点转换到三个特征向量组成的新的XYZ坐标系下,新...
  • OpenGL平面坐标与世界坐标的互转

    千次阅读 2013-08-21 21:04:40
    1、V 表示摄像机的观察矩阵(View Matrix),它的作用是把对象从世界坐标系变换到摄像机坐标系。因此,对于世界坐标系下的坐标值worldCoord(x0, y0, z0),如果希望使用观察矩阵VM 将其变换为摄像机相对坐标系下的...
  • 相机坐标系:以摄像机光心为原点(在针孔模型中也就是针孔为中心),z轴与光轴重合,也就是z轴指向相机的前方(与成像平面垂直),x轴与y轴的正方向与世界坐标系平行,单位为长度单位,比如mm,用矩阵表示;图像物理坐标...
  • 本人刚刚入手学习机器视觉,...将世界坐标系到相机坐标系的转换、相机坐标系转换到图像坐标系(基于小孔成像)的转换、图像坐标系到像素坐标系的转换放到一起 畸变参数(distortion parameter) ·在几何光学和阴极射
  • 1.通过model与view矩阵先将其变换到世界坐标系中,再将其变换到观察坐标系中。从而方便后续处理。 2.经透射投影,将3D坐标映射到视平面,完成从3D到2D的变换,从而在后续可转换为在屏幕显示的2D坐标。 3.透射投影...
  • 本文主要介绍相机针孔模型的基本原理,包括相机坐标系、成像坐标系以及像素坐标系之间的转换关系——内参矩阵(Camera intrinsics),在此基础上介绍该矩阵在MATLAB中的表示以及与其他相关软件中的表示的异同,最后...
  • 因此此时应变矩阵是的函数两个坐标系下坐标转换的桥梁为其中是坐标系中单元四个顶点坐标,是形函数坐标系中任意函数在坐标系的表达式为,根据链式求导法则或者其中,是雅可比矩阵其中由此可得将换成形函数例如在坐标...
  • 首先明确一个问题,全站仪自建坐标系时,无论是使用一个已知点和一个方位角或者是两个已知点建立,其水平方向xy都是跟常识里面的水平方向是一致的。高程是垂直的,可以认为是独立的。 所以明确第二个问题,将载体...
  • 知识点敲黑板,本文需要学习的知识点有坐标系欧拉旋转绕轴旋转旋转矩阵坐标值平移矩阵坐标转换是空间实体的位置描述,是从一种坐标系统变换到另一种坐标系统的过程。通过建立两个坐标系统之间一一对应关系来实现,两...
  • 2D与2.5D坐标转换公式推导

    千次阅读 2015-08-18 13:34:10
    2D与2.5D的关系可以看成X轴与Y轴旋转了指定的角度后形成的新的平面。...所以常规的平面直角坐标系的任何一点可以表示为:  为了将2D坐标映射到2.5坐标,需要定义2.5D坐标系统使用的基向量。因为2.5D坐标系
  • 做完之后,考虑到三维空间的坐标系转换还是不太懂,主要是ROS系统中的tf变换,于是摸了两天鱼。今天详细的记录下来,旋转矩阵、四元素、欧拉角以及各个之间的转换关系。 阅读了好几篇大神的文章,具体也找不到连接了...
  • 如图1.2-1所示,当一个向量沿着...在这儿,为了便于理解,我们定义上述向量都是在二维平面坐标系中,. 下面我们来解释这段话:In component form, the new array can be interpreted as the components of a new v..
  • 任何有坐标的点或者向量乃至平面都是表示在某一坐标系下的,因此在搞清楚某个对象的坐标时先确定它是表示在哪个坐标系下的. slam中经常会遇到欧拉角与旋转矩阵之间的转换,我本人在看代码推公式的时候总是在这一部分...
  • 其中投影矩阵 是一个 的矩阵,可以对其进行分解得到一些属性变量。 投影矩阵 什么时候退化为 ... 如果 3D 世界中的全部一个平面内的点投影到相机成像平面,那么 退化到只剩前两列,因为此时世界坐标系下的点 的纵坐...
  • “世界坐标系、相机坐标系 、归一化平面坐标系、 像平面坐标系、 像素坐标系。”关于相机模型的文章太多了,要搞清楚,首先要弄清楚这5个坐标系,以及他们之间的转换关系。我这里开始由世界坐标系一步一步推导到像素...
  • 标定中各个坐标关系

    2021-01-19 10:08:34
    刚体从世界坐标系转换到相机坐标系的过程,可以通过旋转和平移来得到,我们将其变换矩阵由一个旋转矩阵和平移向量组合成的齐次坐标矩阵。 其中,外参矩阵中R为旋转矩阵,t为平移向量,因为假定在世界坐标系中物点...
  • 矩阵

    2016-11-10 21:17:00
    第一是解线性方程组,比如二维矩阵可以理解为一个平面直角坐标系内的点集,通过计算点与点之间的距离,完成聚类、分类或预测,类似的运算可以扩展到多维的情况。第二个用途是方程降次,也就是利用矩阵的二次型,通过...

空空如也

空空如也

1 2 3 4 5 ... 8
收藏数 149
精华内容 59
关键字:

平面坐标系转换矩阵