【代码】单目摄像机标定程序_分享

guvcolie

首先感谢楼主的代码分享，其中我有几个问题想询问哈。
第一点，在
step=successes*total_per_image; //计算存储相应坐标数据的步长
for(int i=step,j=0;j<total_per_image;++i,++j){
CV_MAT_ELEM(*image_points,float,i,0)=image_points_buf[j].x;
CV_MAT_ELEM(*image_points,float,i,1)=image_points_buf[j].y;
CV_MAT_ELEM(*object_points,float,i,0)=(float)((j/board_width)*square_length);
CV_MAT_ELEM(*object_points,float,i,1)=(float)((j%board_width)*square_height);
中，为什么给object_points赋值的时候是利用j/board_width和j%board_width，还有为什么还要乘以square_length呢，最后为什么好要强制转型为float型呢？
第二点，在
cout<<&quot;按任意键开始计算摄像机内参数...\\n\\n&quot;;


CvCapture* capture1;
capture1=cvCreateCameraCapture(0);
IplImage * show_colie;
show_colie=cvQueryFrame(capture1);

中为什么还要抓取图像一次呢，有什么道理呢
我是新手，希望楼主能够不厌其烦的帮忙解释下哈。呵呵，谢谢了

你好！对于你的第一个问题，请参看本帖第二页有黑白方格的我的那个解释，以前有人问过这个问题。
对于第二个问题，在cout<<&quot;按任意键开始计算摄像机内参数...\\n\\n&quot;;语句前你会发现我有这样的语句
cvReleaseImage(&show);
cvDestroyWindow(&quot;RePlay&quot;);
我已经将之前的获取摄像头的结构体释放掉了，但是我后面又要重新获取图像，所以需要重新定义下！

guvcolie

以下是我的一个项目，关于双目视觉求解三维空间中点的空间坐标的源代码，立体标定、立体校正、单点重映射算法、Kalman滤波器算法、求空间坐标、实时姿态等源码都在其中，我就不一一分类了，有需要的话请大家自行研究！


  /////////////////////////////////////
  样机完成时间2012.12.17
  13:48 pm
  作者：李吉祥
  /////////////////////////////////////
  /////////////////////////////////////
  2012.12.18
  8:27 am对代码进行嵌入Kalman滤波器优化。
  /////////////////////////////////////

#include &quot;stdafx.h&quot;
#include &quot;camerads.h&quot;
#include <highgui.h>
#include <stdio.h>
#include &quot;cv.h&quot;
#include <string>
#include <iostream>

using namespace std;  //添加命名空间，防止冲突

#define TOLERANCE 16  //&quot;同位临点&quot;的容忍度

#define TOL_H 10  //在同一图像中两点认为处于同一水平线的容忍度

#define TOL_EACH 10  //在左右目图像中对应点认为处于同一水平线的容忍度

#define LENGHT_LIMIT 100  //Kalman情况下长度的上下限
#define AREA_LIMIT 1000  //Kalman情况下面积的上下限


//////////////////////////////////////////////////////////////////////////////////////////////
/////////////////Kalman滤波器初始化函数///////////////////////////////////////////////////////
CvKalman * initKalman ( CvKalman * kalman ){

        kalman=cvCreateKalman(8,4,0);//状态变量8维，重心x、y坐标和在x、y方向上的速度，轮廓长度及其变化速度，轮廓面积及其变化速度，
                                                                 //测量变量4维，重心x、y坐标，轮廓长度和面积，
                                                                 //米有控制变量！

        const float F[]={1,0,1,0,0,0,0,0,
                             0,1,0,1,0,0,0,0,
                                         0,0,1,0,0,0,0,0,
                                         0,0,0,1,0,0,0,0,
                                         0,0,0,0,1,1,0,0,
                                         0,0,0,0,0,1,0,0,
                         0,0,0,0,0,0,1,1,
                         0,0,0,0,0,0,0,1};

        memcpy(kalman->transition_matrix->data.fl,F,sizeof(F));

        const float H[]={1,0,0,0,0,0,0,0,
                                         0,1,0,0,0,0,0,0,
                                         0,0,0,0,1,0,0,0,
                                         0,0,0,0,0,0,1,0};

        memcpy(kalman->measurement_matrix->data.fl,H,sizeof(H));
       
        cvSetIdentity( kalman->process_noise_cov, cvScalarAll(1e-5) );//由于我状态变量不是按规则演变的，所以这个没有用！
        cvSetIdentity( kalman->measurement_noise_cov, cvScalarAll(1e-5) );//我能精确观察，所以测量噪声协方差也可以不用了！
        cvSetIdentity( kalman->error_cov_post, cvScalarAll(1));  //这个真心不好解释啊，看书吧~~~╭(╯^╰)╮

        return kalman;

}
/////////////////Kalman滤波器初始化函数结束///////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////


struct Vertex {  //三角形顶点（标志点）结构体
        int usefull;
        CvPoint point1;
        CvPoint point2;
        CvPoint point3;
};


//////////////////////////////////////////////////////////////////////////////////////////////
////////////////获取三角形顶点（标志点跟踪算法）//////////////////////////////////////////////
Vertex getTriangleVertex(CvSeq* contour){

        Vertex vt = { 0,{0,0},};  //初始化三个标志点横纵坐标均为（0,0）
        CvSeq * ct;  //用来接收传入的参数
        ct = contour;

        CvPoint * p_minX_1 = 0;  //保存“横纵极值”的点
        CvPoint * p_minX_2 = 0;  //保存“同位临点”
        CvPoint * p_maxX_1 = 0;
        CvPoint * p_maxX_2 = 0;
        CvPoint * p_minY_1 = 0;
        CvPoint * p_minY_2 = 0;
        CvPoint * p_maxY_1 = 0;
        CvPoint * p_maxY_2 = 0;

        int maxX_x,maxX_y,minX_x,minX_y,maxY_x,maxY_y,minY_x,minY_y;  //保存横纵极值的X、Y坐标

        CvPoint * p1 = ( CvPoint * ) cvGetSeqElem ( ct , 0 );  //取轮廓序列的第1、2个点，作为初始值
        CvPoint * p2 = ( CvPoint * ) cvGetSeqElem ( ct , 1 );
        if ( p1->x >= p2->x ){  //以下的两个if—else是将第1、2点初始化填入“横纵极值”点钟
                p_maxX_1 = p1;
                p_minX_1 = p2;
        }else{
                p_maxX_1 = p2;
                p_minX_1 = p1;
        }
        if ( p1->y >= p2->y ){
                p_maxY_1 = p1;
                p_minY_1 = p2;
        }else{
                p_maxY_1 = p2;
                p_minY_1 = p1;
        }
       
       
        for(int i=2;i<ct->total;i++){  //以下的for循环是找到三角形轮廓的“横纵极值”点
                CvPoint* p = (CvPoint *)cvGetSeqElem( ct, i );
                if ( p->x >= p_maxX_1->x){
                        if(p->x == p_maxX_1->x){
                                p_maxX_2 = p;
                        }else{
                                p_maxX_1 = p;
                                p_maxX_2 = 0;
                        }
                }else{
                        if(p->x <= p_minX_1->x){
                                if(p->x == p_maxX_1->x){
                                        p_minX_2 = p;
                                }else{
                                        p_minX_1 = p;
                                        p_minX_2 = 0;
                                }
                        }
                }

                if ( p->y >= p_maxY_1->y){
                        if(p->y == p_maxY_1->y){
                                p_maxY_2 = p;
                        }else{
                                p_maxY_1 = p;
                                p_maxY_2 = 0;
                        }
                }else{
                        if(p->y <= p_minY_1->y){
                                if(p->y == p_maxY_1->y){
                                        p_minY_2 = p;
                                }else{
                                        p_minY_1 = p;
                                        p_minY_2 = 0;
                                }
                        }
                }

        }
       
       
        if(p_maxX_2){
                maxX_x = p_maxX_2->x;
                maxX_y = (p_maxX_1->y + p_maxX_2->y)/2;
        }else{
                maxX_x = p_maxX_1->x;
                maxX_y = p_maxX_1->y;
        }


        if(p_minX_2){
                minX_x = p_minX_2->x;
                minX_y = (p_minX_1->y + p_maxX_2->y)/2;
        }else{
                minX_x = p_minX_1->x;
                minX_y = p_minX_1->y;
        }


        if(p_maxY_2){
                maxY_x = (p_maxY_1->x + p_maxY_2->x)/2;
                maxY_y = p_maxY_2->y;
        }else{
                maxY_x = p_maxY_1->x;
                maxY_y = p_maxY_1->y;
        }


        if(p_minY_2){
                minY_x = (p_minY_1->x + p_minY_2->x)/2;
                minY_y = p_minY_2->y;
        }else{
                minY_x = p_minY_1->x;
                minY_y = p_minY_1->y;
        }
       


        /*下面这4个if语句用于处理三角形的一边平行X或者Y轴的情况 2012-11-8 14:15*/
        /*例如若一边平行X轴，假设在Y方向较大的一侧，此时p_maxY_2存在，但是p_maxY_1和p_maxY_2的横坐标会相差很大，
          就是要利用这个非常大的特点，来对这种特殊情况进行特殊处理！！ ：-）*/
        if(p_maxX_2 && fabs((float)(p_maxX_1->y - p_maxX_2->y))>= 30){
                vt.point1.x = minX_x;
                vt.point1.y = minX_y;
                vt.point2.x = maxY_x;
                vt.point2.y = maxY_y;
                vt.point3.x = minY_x;
                vt.point3.y = minY_y;
                vt.usefull = 1;
                return vt;
        }
        if(p_minX_2 && fabs((float)(p_minX_1->y - p_minX_2->y))>= 30){
                vt.point1.x = maxX_x;
                vt.point1.y = maxX_y;
                vt.point2.x = maxY_x;
                vt.point2.y = maxY_y;
                vt.point3.x = minY_x;
                vt.point3.y = minY_y;
                vt.usefull = 1;
                return vt;
        }
        if(p_maxY_2 && fabs((float)(p_maxY_1->x - p_maxY_2->x))>= 30){
                vt.point1.x = minX_x;
                vt.point1.y = minX_y;
                vt.point2.x = maxX_x;
                vt.point2.y = maxX_y;
                vt.point3.x = minY_x;
                vt.point3.y = minY_y;
                vt.usefull = 1;
                return vt;
        }
        if(p_minY_2 && fabs((float)(p_minY_1->x - p_minY_2->x))>= 30){
                vt.point1.x = minX_x;
                vt.point1.y = minX_y;
                vt.point2.x = maxY_x;
                vt.point2.y = maxY_y;
                vt.point3.x = maxX_x;
                vt.point3.y = maxX_y;
                vt.usefull = 1;
                return vt;
        }


        /**********以下是要根据“横纵极值”点找到三个标志点*********/
        //这个算法有待优化！这里只是找“同位临点”和对应“横纵极值”点的中点！ O(∩_∩)O~
        if( fabs((float)(maxX_x - maxY_x)) <= TOLERANCE && fabs((float)(maxX_y - maxY_y)) <= TOLERANCE){
                vt.point1.x = (maxX_x + maxY_x)/2;
                vt.point1.y = (maxX_y + maxY_y)/2;
                vt.point2.x = minX_x;
                vt.point2.y = minX_y;
                vt.point3.x = minY_x;
                vt.point3.y = minY_y;
                vt.usefull = 1;
                return vt;
        }
        if( fabs((float)(maxX_x - minY_x)) <= TOLERANCE && fabs((float)(maxX_y - minY_y)) <= TOLERANCE){
                vt.point1.x = (maxX_x + minY_x)/2;
                vt.point1.y = (maxX_y + minY_y)/2;
                vt.point2.x = minX_x;
                vt.point2.y = minX_y;
                vt.point3.x = maxY_x;
                vt.point3.y = maxY_y;
                vt.usefull = 1;
                return vt;
        }
        if( fabs((float)(minX_x - maxY_x)) <= TOLERANCE && fabs((float)(minX_y - maxY_y)) <= TOLERANCE){
                vt.point1.x = (minX_x + maxY_x)/2;
                vt.point1.y = (minX_y + maxY_y)/2;
                vt.point2.x = maxX_x;
                vt.point2.y = maxX_y;
                vt.point3.x = minY_x;
                vt.point3.y = minY_y;
                vt.usefull = 1;
                return vt;
        }
        if( fabs((float)(minX_x - minY_x)) <= TOLERANCE && fabs((float)(minX_y - minY_y)) <= TOLERANCE){
                vt.point1.x = (minX_x + minY_x)/2;
                vt.point1.y = (minX_y + minY_y)/2;
                vt.point2.x = maxX_x;
                vt.point2.y = maxX_y;
                vt.point3.x = maxY_x;
                vt.point3.y = maxY_y;
                vt.usefull = 1;
                return vt;
        }
       
        //程序如果运行到这里，说明我没有利用分离出来的三角形轮廓找到顶点（标志点）！~~~~(>_<)~~~~
        vt.usefull = 0;  //设置为不可用，为后续程序判断是否该函数找到了标志点使用！╭(╯^╰)╮
        return vt;
       
}
//////////////////////////////getTriangleVertex至此结束///////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////左右目图像中标志点对应（标志点匹配算法）////////////////////////
int sortVertexs (Vertex * vertex1, Vertex * vertex2){

        Vertex tmp_vt1 = { 0,{0,0},};  //临时变量，初始化三个标志点横纵坐标均为（0,0）
        Vertex tmp_vt2 = { 0,{0,0},};  //初始化三个标志点横纵坐标均为（0,0）
        CvPoint tmp_point;  //用于两个标志点互换时的中转变量

        int is_match = 0;
       
        tmp_vt1 = * vertex1;  //复制拷贝
        tmp_vt2 = * vertex2;

        /*以下的5个if语句对左图像的三个标志点进行排序，
          先按Y方向从上到下排序，
          再按X方向从左到右排序*/
        //以下对左图像的三个标志点按Y方向降序排列
        if (tmp_vt1.point1.y > tmp_vt1.point2.y){
                tmp_point = tmp_vt1.point1;
                tmp_vt1.point1 = tmp_vt1.point2;
                tmp_vt1.point2 = tmp_point;
        }
        if (tmp_vt1.point1.y > tmp_vt1.point3.y){
                tmp_point = tmp_vt1.point1;
                tmp_vt1.point1 = tmp_vt1.point3;
                tmp_vt1.point3 = tmp_point;
        }
        if (tmp_vt1.point2.y > tmp_vt1.point3.y){
                tmp_point = tmp_vt1.point2;
                tmp_vt1.point2 = tmp_vt1.point3;
                tmp_vt1.point3 = tmp_point;
        }
        //以下对存在水平排列的两个标志点的情况进行再精细排列
        if (fabs((float)(tmp_vt1.point1.y-tmp_vt1.point2.y)) <= TOL_H){
                if (tmp_vt1.point1.x > tmp_vt1.point2.x){
                        tmp_point = tmp_vt1.point1;
                        tmp_vt1.point1 = tmp_vt1.point2;
                        tmp_vt1.point2 = tmp_point;
                }
        }
        if (fabs((float)(tmp_vt1.point2.y-tmp_vt1.point3.y)) <= TOL_H){
                if (tmp_vt1.point2.x > tmp_vt1.point3.x){
                        tmp_point = tmp_vt1.point2;
                        tmp_vt1.point2 = tmp_vt1.point3;
                        tmp_vt1.point3 = tmp_point;
                }
        }


        /*以下的5个if语句对右图像的三个标志点进行排序，
          先按Y方向从上到下排序，
          再按X方向从左到右排序*/
        //以下对右图像的三个标志点按Y方向降序排列
        if (tmp_vt2.point1.y > tmp_vt2.point2.y){
                tmp_point = tmp_vt2.point1;
                tmp_vt2.point1 = tmp_vt2.point2;
                tmp_vt2.point2 = tmp_point;
        }
        if (tmp_vt2.point1.y > tmp_vt2.point3.y){
                tmp_point = tmp_vt2.point1;
                tmp_vt2.point1 = tmp_vt2.point3;
                tmp_vt2.point3 = tmp_point;
        }
        if (tmp_vt2.point2.y > tmp_vt2.point3.y){
                tmp_point = tmp_vt2.point2;
                tmp_vt2.point2 = tmp_vt2.point3;
                tmp_vt2.point3 = tmp_point;
        }
        //以下的2个if对存在水平排列的两个标志点的情况进行再精细排列
        if (fabs((float)(tmp_vt2.point1.y-tmp_vt2.point2.y)) <= TOL_H){
                if (tmp_vt2.point1.x > tmp_vt2.point2.x){
                        tmp_point = tmp_vt2.point1;
                        tmp_vt2.point1 = tmp_vt2.point2;
                        tmp_vt2.point2 = tmp_point;
                }
        }
        if (fabs((float)(tmp_vt2.point2.y-tmp_vt2.point3.y)) <= TOL_H){
                if (tmp_vt2.point2.x > tmp_vt2.point3.x){
                        tmp_point = tmp_vt2.point2;
                        tmp_vt2.point2 = tmp_vt2.point3;
                        tmp_vt2.point3 = tmp_point;
                }
        }


        //判断左右目中的标志点是否具备精度要求相互匹配
        if (fabs((float)(tmp_vt1.point1.y - tmp_vt2.point1.y)) > TOL_EACH ||
                fabs((float)(tmp_vt1.point2.y - tmp_vt2.point2.y)) > TOL_EACH ||
                fabs((float)(tmp_vt1.point3.y - tmp_vt2.point3.y)) > TOL_EACH){
                        is_match = 0;
                        return is_match;
        }

        * vertex1 = tmp_vt1;
        * vertex2 = tmp_vt2;
        is_match = 1;
        return is_match;


}
//////////////////////////////sortVertexs至此结束//////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////



/////////////////////////////////////main函数从这里开始////////////////////////////////////////
int main()
{

        int cam_count;

        IplImage*    g_gray    = NULL;  //存放灰度图像
        IplImage*    g_gray1    = NULL;  //存放灰度图像

        int g_thresh  = 115;  //阈值
        int g_thresh1  = 115;  //阈值

        CvMemStorage*  g_storage  = NULL;  //内存
        CvSeq * contours = 0;  //序列
        CvSeq * contour_triangle = 0;
        CvContourScanner scanner = NULL;
        CvMemStorage*  g_storage1  = NULL;  //内存
        CvSeq * contours1 = 0;  //序列
        CvSeq * contour_triangle1 = 0;
        CvContourScanner scanner1 = NULL;

        double min_lenght =200.0;  //分离轮廓的长度阈值
        double max_lenght =700.0;
        double tmp_lenght = 0.0;

        double min_area = 6000.0;  //分离轮廓的面积阈值
        double max_area = 14000.0;
        double tmp_area = 0.0;

        double tmp_lenght1 = 0.0;
        double tmp_area1 = 0.0;

        Vertex vertex;  //标志点结构体
        int heart_X;  // 重心X坐标
        int heart_Y;  //重心Y坐标
        int lenght_triangle;  //三角形轮廓长度
        int area_triangle;  //三角形轮廓面积
        int numberOfContour = 0;  //记录每帧一共截取出来几个轮廓.
        int is_getVertexAtLeft;  //左图像获取三角形标志点的判断符

        Vertex vertex1;  //标志点结构体
        int heart_X1;  // 重心X坐标
        int heart_Y1;  //重心Y坐标
        int lenght_triangle1;  //三角形轮廓长度
        int area_triangle1;  //三角形轮廓面积
        int numberOfContour1 = 0;  //记录每帧一共截取出来几个轮廓.
        int is_getVertexAtRight;  //右图像获取三角形标志点的判断符

        int is_LRmatched;  //左右目图像中标志点匹配标志位

        float Tx;  //左右目相机之间的距离
        int d;  //左右目视差
        float Cx;  //立体标定后主点的图像横坐标
        float Cy;  //立体标定后主点的图像纵坐标
        float f;  //相机焦距

        char point1[40]=&quot;&quot;;   //保存坐标的字符串数组
        char point2[40]=&quot;&quot;;   //保存坐标的字符串数组
        char point3[40]=&quot;&quot;;   //保存坐标的字符串数组

        char point11[40]=&quot;&quot;;   //保存坐标的字符串数组
        char point21[40]=&quot;&quot;;   //保存坐标的字符串数组
        char point31[40]=&quot;&quot;;   //保存坐标的字符串数组

        char object_point1[50] = &quot;&quot;;  //保存三维空间坐标的字符串数组
        char object_point2[50] = &quot;&quot;;
        char object_point3[50] = &quot;&quot;;

        char angle_FW[50] = &quot;&quot;;  //保存方位角及俯仰角的字符串数组
        char angle_FY[50] = &quot;&quot;;

        cam_count = CCameraDS::CameraCount();
        printf(&quot;There are %d cameras.\\n&quot;, cam_count);

        //获取所有摄像头的名称
        for(int i=0; i < cam_count; i++){
                char camera_name[1024];  
                int retval = CCameraDS::CameraName(i, camera_name, sizeof(camera_name) );

                if(retval >0)
                        printf(&quot;Camera #%d\'s Name is \'%s\'.\\n&quot;, i, camera_name);
                else
                        printf(&quot;Can not get Camera #%d\'s name.\\n&quot;, i);
        }

        if(cam_count==0){
                return -1;
        }

        CCameraDS camera;
        CCameraDS camera1;
        //打开第一个摄像头
        if(! camera.OpenCamera(0, true)) //弹出属性选择窗口
        {
                fprintf(stderr, &quot;Can not open camera.\\n&quot;);
                return -1;
        }
        //打开第二个摄像头
        if(! camera1.OpenCamera(1, true)) //弹出属性选择窗口
        {
                fprintf(stderr, &quot;Can not open camera1.\\n&quot;);
                return -1;
        }

        IplImage * copy = cvCreateImage(cvSize(1024,768),8,3);  //以后用作原始彩色图像的拷贝
        IplImage * copy1 = cvCreateImage(cvSize(1024,768),8,3);  //以后用作原始彩色图像的拷贝

        IplImage * images_combined = cvCreateImage(cvSize(2048, 768), 8, 3);

        CvFont font;
        cvInitFont( &font , CV_FONT_HERSHEY_SIMPLEX , 1.0 , 1.0 , 0 , 5 );  //创建字体风格

        cvNamedWindow(&quot;轮廓图像&quot;);  //为轮廓图像创建显示窗口
        cvNamedWindow(&quot;阈值化图像&quot;);  //为阈值化图像创建窗口
        cvNamedWindow(&quot;轮廓图像1&quot;);  //为轮廓图像创建显示窗口
        cvNamedWindow(&quot;阈值化图像1&quot;);  //为阈值化图像创建窗口


        cvCreateTrackbar(&quot;阈值&quot;,&quot;阈值化图像&quot;,&g_thresh,255,NULL);  //绑定设置阈值的滚动条
        cvCreateTrackbar(&quot;阈值&quot;,&quot;阈值化图像1&quot;,&g_thresh1,255,NULL);  //绑定设置阈值的滚动条

        IplImage * frame;   //图像指针
        IplImage * frame1;

        IplImage * image = cvCreateImage(cvSize(1024,768),8,3);
        IplImage * image1 = cvCreateImage(cvSize(1024,768),8,3);

        IplImage * gray = cvCreateImage(cvSize(1024,768),8,1);
        IplImage * gray1 = cvCreateImage(cvSize(1024,768),8,1);

        cvNamedWindow (&quot;合并图像窗口&quot;);


        CvSize imageSize = cvSize (1024,768);

        CvMat * R = cvCreateMat (3,3,CV_64FC1);  //旋转矩阵
        CvMat * T = cvCreateMat (3,1,CV_64FC1);  //平移矩阵


        CvMat * object_points = (CvMat *) cvLoad (&quot;Object_points.xml&quot;);  //以下将单目标定的结果矩阵调入，用作立体标定
        CvMat * left_points = (CvMat *) cvLoad (&quot;Left_points.xml&quot;);
        CvMat * right_points = (CvMat *) cvLoad (&quot;Right_points.xml&quot;);
        CvMat * counts = (CvMat *) cvLoad (&quot;Counts.xml&quot;);
        CvMat * left_intrinsic = (CvMat *) cvLoad (&quot;Left_intrinsic.xml&quot;);
        CvMat * left_distortion = (CvMat *) cvLoad (&quot;Left_distortion.xml&quot;);
        CvMat * right_intrinsic = (CvMat *) cvLoad (&quot;Right_intrinsic.xml&quot;);
        CvMat * right_distortion = (CvMat *) cvLoad (&quot;Right_distortion.xml&quot;);

        cvStereoCalibrate ( object_points, left_points, right_points, counts,  //立体标定
                                            left_intrinsic, left_distortion,
                                                right_intrinsic, right_distortion,
                                                imageSize,
                                                R, T, 0, 0,
                                                cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
                                                CV_CALIB_FIX_ASPECT_RATIO+CV_CALIB_FIX_PRINCIPAL_POINT+CV_CALIB_SAME_FOCAL_LENGTH
                                                );
       
        Tx = -51.4346;

        cvSave (&quot;R.xml&quot;,R);
        cvSave (&quot;T.xml&quot;,T);
//        T = (CvMat *) cvLoad (&quot;T.xml&quot;);
//        Tx = CV_MAT_ELEM( *T, float, 0, 0);

        CvMat * mx1 = cvCreateMat (imageSize.height, imageSize.width, CV_32FC1);
        CvMat * my1 = cvCreateMat (imageSize.height, imageSize.width, CV_32FC1);
        CvMat * mx2 = cvCreateMat (imageSize.height, imageSize.width, CV_32FC1);
        CvMat * my2 = cvCreateMat (imageSize.height, imageSize.width, CV_32FC1);

        CvMat * R1 = cvCreateMat (3,3,CV_64FC1);
        CvMat * R2 = cvCreateMat (3,3,CV_64FC1);
        CvMat * P1 = cvCreateMat (3,4,CV_64FC1);
        CvMat * P2 = cvCreateMat (3,4,CV_64FC1);
        CvMat * Q = cvCreateMat (4, 4, CV_64FC1);

        cvStereoRectify ( left_intrinsic, right_intrinsic,  //立体校正
                                          left_distortion, right_distortion,
                                          imageSize,
                                          R, T,
                                          R1, R2, P1, P2,
                                          Q, 0
                                         );

//        Cx = CV_MAT_ELEM(* Q, float, 0, 3);
//        Cy = CV_MAT_ELEM(* Q, float, 1, 3);
//        f = CV_MAT_ELEM(* Q, float, 2, 3);

        Cx = 603.4027;
        Cy = 381.0301;
        f = 1867.8709;

        cvSave (&quot;Q.xml&quot;, Q);

        cvInitUndistortRectifyMap (left_intrinsic, left_distortion, R1, P1, mx1, my1);  //计算映射矩阵
        cvInitUndistortRectifyMap (right_intrinsic, right_distortion, R2, P2, mx2, my2);

        cvSave (&quot;mapx.xml&quot;, mx1);
        cvSave (&quot;mapy.xml&quot;, my1);
        cvSave (&quot;mapx1.xml&quot;, mx2);
        cvSave (&quot;mapy1.xml&quot;, my2);

        while (true){

                if ( cvWaitKey( 10 ) == \'k\'){  //当按下“K”键时，将嵌入Kalman滤波器
                        break;  //OK！此时说明我定位出了要跟踪的三角形了，现在将跳出这个大循环，进入嵌入Kalman的大循环！(*^__^*)
                }
               
                frame=camera.QueryFrame();
                frame1=camera1.QueryFrame();

                if (frame && frame1){
                        cvRemap (frame, image, mx1, my1);
                        cvRemap (frame1, image1, mx2, my2);
                }

                cvCopy(image,copy);   //复制每帧原始图像，前期调试阶段使用，后期可考虑删除相关代码！O(∩_∩)O~
                cvCopy(image1,copy1);   //复制每帧原始图像，前期调试阶段使用，后期可考虑删除相关代码！O(∩_∩)O~

                if( g_storage==NULL ) {  //
                        g_gray = cvCreateImage( cvGetSize(image), 8, 1 );
                           g_storage = cvCreateMemStorage(0);
                  } else {
                    cvClearMemStorage( g_storage );
                }

                if( g_storage1==NULL ) {  //
                        g_gray1 = cvCreateImage( cvGetSize(image1), 8, 1 );
                           g_storage1 = cvCreateMemStorage(0);
                  } else {
                    cvClearMemStorage( g_storage1 );
                }

                 
                cvCvtColor( image, g_gray, CV_BGR2GRAY );  //将原始彩色图像转换为灰度图像
                cvCvtColor( image1, g_gray1, CV_BGR2GRAY );  //将原始彩色图像转换为灰度图像

                cvThreshold( g_gray, g_gray, g_thresh, 255, CV_THRESH_BINARY );  //根据拖动滚动条，对灰度图像进行阈值化
                cvThreshold( g_gray1, g_gray1, g_thresh1, 255, CV_THRESH_BINARY );  //根据拖动滚动条，对灰度图像进行阈值化

                cvShowImage(&quot;阈值化图像&quot;,g_gray);
                cvShowImage(&quot;阈值化图像1&quot;,g_gray1);

                scanner = cvStartFindContours(g_gray,g_storage,sizeof(CvContour),CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE,cvPoint(0,0));
                scanner1 = cvStartFindContours(g_gray1,g_storage1,sizeof(CvContour),CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE,cvPoint(0,0));

                is_getVertexAtLeft = 0;
                is_getVertexAtRight = 0;

                is_LRmatched = 0;


                /*************以下while遍历左图像每个轮廓，分离出三角形的轮廓！*************/
                numberOfContour=0;  //记录每帧一共截取出来几个轮廓（正常情况应该就出现1个轮廓！）
                                                        //如果发现下面的循环之后numberOfContour大于1，好吧，我们发现了冒牌轮廓 ~~~~(>_<)~~~~
                contour_triangle = 0;  //每次循环之前，要清理上次的垃圾信息！

                while (contours = cvFindNextContour(scanner)){
                       
                        tmp_lenght = fabs(cvContourPerimeter(contours));  //计算一个轮廓的长度
                        if (tmp_lenght < min_lenght || tmp_lenght > max_lenght){
                                cvSubstituteContour(scanner,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }
                       
                        tmp_area = fabs(cvContourArea(contours));  //计算一个轮廓的长度       
                        if (tmp_area < min_area || tmp_area > max_area){
                                cvSubstituteContour(scanner,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }

                        numberOfContour++;  //发现一个疑似三角形，就自加1
                       
                        contour_triangle = contours;  //将“准三角形轮廓”拷贝，退出这个循环后我再慢慢拷问它是不是真的 ╭(╯^╰)╮

                        lenght_triangle = tmp_lenght;  //下面两个值要保存下来，当切入Kalman循环时这两个值会作为状态变量的初始值！

                        area_triangle = tmp_area;

                }
               

                //下面这个if-else就是要严刑拷问“准三角形轮廓”的真身的！
                /*如果没得到“准三角形轮廓”（系统刚启动时阈值化调得不理想或者后期的的确确没找到像三角形的轮廓），那我们就没必要在图像上和控制台上显示
                坐标了！如果contour_triangle不为空但是numberOfContour大于1，说明有假冒的“三角形轮廓”混进来了，既然它混进来了，那我就也不显示坐标了！*/
                if ( contour_triangle &&  numberOfContour == 1 ){  

                        vertex = getTriangleVertex( contour_triangle );  //获取三角形轮廓的顶点（标志点）

                        if ( vertex.usefull != 0 ){  //判断是否利用分离出来的三角形成功获取到了标志点
                                heart_X = ( vertex.point1.x + vertex.point2.x + vertex.point3.x )/3;  //计算重心坐标
                                heart_Y = ( vertex.point1.y + vertex.point2.y + vertex.point3.y )/3;
                                cvLine( copy , cvPoint(vertex.point1.x,vertex.point1.y) , cvPoint(heart_X,heart_Y) , cvScalar(255,255,255) , 1 , 8 );  //绘制三条中线
                                cvLine( copy , cvPoint(vertex.point2.x,vertex.point2.y) , cvPoint(heart_X,heart_Y) , cvScalar(255,255,255) , 1 , 8 );
                                cvLine( copy , cvPoint(vertex.point3.x,vertex.point3.y) , cvPoint(heart_X,heart_Y) , cvScalar(255,255,255) , 1 , 8 );
                                cvCircle( copy , cvPoint(vertex.point1.x,vertex.point1.y) , 1 , cvScalar(0,0,255) , 5 , 8 );  //在彩色图像上标记标志点
                                cvCircle( copy , cvPoint(vertex.point2.x,vertex.point2.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy , cvPoint(vertex.point3.x,vertex.point3.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy , cvPoint(heart_X,heart_Y) , 1 , cvScalar(255,255,255) , 5 , 8 );

                                is_getVertexAtLeft = 1;  //左图像获取到了三角形标志点                               
/*
                                sprintf_s (point1,&quot;<%d   %d>&quot;,vertex.point1.x , vertex.point1.y);  //在彩色图像左上角显示标志点坐标
                                cvPutText( copy , point1 , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                sprintf_s (point2,&quot;<%d   %d>&quot;,vertex.point2.x , vertex.point2.y);
                                cvPutText( copy , point2 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                                sprintf_s (point3,&quot;<%d   %d>&quot;,vertex.point3.x , vertex.point3.y);
                                cvPutText( copy , point3 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
*/
                                cvPutText( copy , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );

                        }else{
                                is_getVertexAtLeft = 0;  //左图像未获取到三角形标志点
                                cvPutText( copy , &quot;Fail to get vertex!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );
                        }
                }else{
                        if( !contour_triangle ){  //此时是米有找到符合条件的轮廓
                                is_getVertexAtLeft = 0;  //左图像未获取到三角形标志点
                                cvPutText( copy , &quot;Fail to get contour of the triangle!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );
                        }else if( numberOfContour > 1 ){  //此时是找到了多于1个三角形的轮廓
                                is_getVertexAtLeft = 0;  //左图像未获取到三角形标志点
                                cvDrawContours( copy ,contour_triangle ,cvScalar(255,255,0),cvScalar(255,255,0),2,3,8,cvPoint(0,0));
                                cvPutText( copy , &quot;Find one more like-trianglecontour!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );
                        }
                }


                contours = cvEndFindContours(&scanner);

                cvZero( g_gray );  //将阈值化图像清空，也就是一个黑屏，当作下面的画布
               
                if( contours){  //如果有轮廓信息，就在画布上绘制出轮廓   
                        cvDrawContours(g_gray,contours,cvScalarAll(255),cvScalarAll(255),2,1,8,cvPoint(0,0));
                }

//                if (is_getVertexAtLeft == 0){
//                        continue;
//                }


                /////////////////////////////////////////////////////////////////////////////////////////////
                /////////////////////////////////////////////////////////////////////////////////////////////
                /*************以下while遍历右图像每个轮廓，分离出三角形的轮廓！*************/
                numberOfContour1=0;  //记录每帧一共截取出来几个轮廓（正常情况应该就出现1个轮廓！）
                                                        //如果发现下面的循环之后numberOfContour大于1，好吧，我们发现了冒牌轮廓 ~~~~(>_<)~~~~
                contour_triangle1 = 0;  //每次循环之前，要清理上次的垃圾信息！

                while (contours1 = cvFindNextContour(scanner1)){
                       
                        tmp_lenght1 = fabs(cvContourPerimeter(contours1));  //计算一个轮廓的长度
                        if (tmp_lenght1 < min_lenght || tmp_lenght1 > max_lenght){
                                cvSubstituteContour(scanner1,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }
                       
                        tmp_area1 = fabs(cvContourArea(contours1));  //计算一个轮廓的长度       
                        if (tmp_area1 < min_area || tmp_area1 > max_area){
                                cvSubstituteContour(scanner1,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }

                        numberOfContour1++;  //发现一个疑似三角形，就自加1
                       
                        contour_triangle1 = contours1;  //将“准三角形轮廓”拷贝，退出这个循环后我再慢慢拷问它是不是真的 ╭(╯^╰)╮

                        lenght_triangle1 = tmp_lenght1;  //下面两个值要保存下来，当切入Kalman循环时这两个值会作为状态变量的初始值！

                        area_triangle1 = tmp_area1;

                }
               

                //下面这个if-else就是要严刑拷问“准三角形轮廓”的真身的！
                /*如果没得到“准三角形轮廓”（系统刚启动时阈值化调得不理想或者后期的的确确没找到像三角形的轮廓），那我们就没必要在图像上和控制台上显示
                坐标了！如果contour_triangle不为空但是numberOfContour大于1，说明有假冒的“三角形轮廓”混进来了，既然它混进来了，那我就也不显示坐标了！*/
                if ( contour_triangle1 &&  numberOfContour1 == 1 ){  

                        vertex1 = getTriangleVertex( contour_triangle1 );  //获取三角形轮廓的顶点（标志点）

                        if ( vertex1.usefull != 0 ){  //判断是否利用分离出来的三角形成功获取到了标志点
                                heart_X1 = ( vertex1.point1.x + vertex1.point2.x + vertex1.point3.x )/3;  //计算重心坐标
                                heart_Y1 = ( vertex1.point1.y + vertex1.point2.y + vertex1.point3.y )/3;
                                cvLine( copy1 , cvPoint(vertex1.point1.x,vertex1.point1.y) , cvPoint(heart_X1,heart_Y1) , cvScalar(255,255,255) , 1 , 8 );  //绘制三条中线
                                cvLine( copy1 , cvPoint(vertex1.point2.x,vertex1.point2.y) , cvPoint(heart_X1,heart_Y1) , cvScalar(255,255,255) , 1 , 8 );
                                cvLine( copy1 , cvPoint(vertex1.point3.x,vertex1.point3.y) , cvPoint(heart_X1,heart_Y1) , cvScalar(255,255,255) , 1 , 8 );
                                cvCircle( copy1 , cvPoint(vertex1.point1.x,vertex1.point1.y) , 1 , cvScalar(0,0,255) , 5 , 8 );  //在彩色图像上标记标志点
                                cvCircle( copy1 , cvPoint(vertex1.point2.x,vertex1.point2.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy1 , cvPoint(vertex1.point3.x,vertex1.point3.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy1 , cvPoint(heart_X1,heart_Y1) , 1 , cvScalar(255,255,255) , 5 , 8 );

                                is_getVertexAtRight = 1;  //右图像获取到了三角形标志点
                               
/*
                                sprintf_s (point11,&quot;<%d   %d>&quot;,vertex1.point1.x , vertex1.point1.y);  //在彩色图像左上角显示标志点坐标
                                cvPutText( copy1 , point11 , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                sprintf_s (point21,&quot;<%d   %d>&quot;,vertex1.point2.x , vertex1.point2.y);
                                cvPutText( copy1 , point21 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                                sprintf_s (point31,&quot;<%d   %d>&quot;,vertex1.point3.x , vertex1.point3.y);
                                cvPutText( copy1 , point31 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
*/
                                cvPutText( copy1 , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );

                        }else{
                                is_getVertexAtRight = 0;  //右图像未获取到三角形标志点
                                cvPutText( copy1 , &quot;Fail to get vertex!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy1 , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );
                        }

                }else{
                        if( !contour_triangle1 ){  //此时是米有找到符合条件的轮廓
                                is_getVertexAtRight = 0;  //右图像未获取到三角形标志点
                                cvPutText( copy1 , &quot;Fail to get contour of the triangle!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy1 , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );
                        }else if( numberOfContour1 > 1 ){  //此时是找到了多于1个三角形的轮廓
                                is_getVertexAtRight = 0;  //右图像未获取到三角形标志点
                                cvDrawContours( copy1 ,contour_triangle1 ,cvScalar(255,255,0),cvScalar(255,255,0),2,3,8,cvPoint(0,0));
                                cvPutText( copy1 , &quot;Find one more like-trianglecontour!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy1 , &quot;Kalman is powered off&quot; , cvPoint(600,40) , &font , cvScalar(0,0,255) );
                        }
                }


                contours1 = cvEndFindContours(&scanner1);

                cvZero( g_gray1 );  //将阈值化图像清空，也就是一个黑屏，当作下面的画布
               
                if( contours1){  //如果有轮廓信息，就在画布上绘制出轮廓   
                        cvDrawContours(g_gray1,contours1,cvScalarAll(255),cvScalarAll(255),2,1,8,cvPoint(0,0));
                }
                /////////////////////////////////////////////////////////////////////////////////////////////
                /////////////////////////////////////////////////////////////////////////////////////////////
                 

                if (is_getVertexAtLeft == 1 && is_getVertexAtRight == 1){

                        int x;  //左目一个标志点的横坐标
                        int y;  //左目一个标志点的纵坐标
                        double X1, Y1, Z1;  //第一个标志点的空间三维坐标
                        double X2, Y2, Z2;  //第二个标志点的空间三维坐标
                        double X3, Y3, Z3;  //第三个标志点的空间三维坐标

                        double n1x, n1y, n1z;  //第一个向量
                        double n2x, n2y, n2z;  //第二个向量

                        double ve_x, ve_y, ve_z;  //三角形所在平面的空间法向量

                        double angle_fy, angle_fw;  //俯仰角、方位角
                       
                        //cvZero (images_combined);

                        is_LRmatched = sortVertexs( & vertex, & vertex1);  //左右目标志点匹配函数

                        cvPutText( copy , &quot;Image Coordinate:&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                        sprintf_s (point1,&quot;<%d   %d>&quot;,vertex.point1.x , vertex.point1.y);  //在彩色图像左上角显示标志点图象坐标
                        cvPutText( copy , point1 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                        sprintf_s (point2,&quot;<%d   %d>&quot;,vertex.point2.x , vertex.point2.y);
                        cvPutText( copy , point2 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
                        sprintf_s (point3,&quot;<%d   %d>&quot;,vertex.point3.x , vertex.point3.y);
                        cvPutText( copy , point3 , cvPoint(40,160) , &font , cvScalar(0,0,255) );

                        cvPutText( copy1 , &quot;Image Coordinate:&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                        sprintf_s (point11,&quot;<%d   %d>&quot;,vertex1.point1.x , vertex1.point1.y);  //在彩色图像左上角显示标志点图象坐标
                        cvPutText( copy1 , point11 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                        sprintf_s (point21,&quot;<%d   %d>&quot;,vertex1.point2.x , vertex1.point2.y);
                        cvPutText( copy1 , point21 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
                        sprintf_s (point31,&quot;<%d   %d>&quot;,vertex1.point3.x , vertex1.point3.y);
                        cvPutText( copy1 , point31 , cvPoint(40,160) , &font , cvScalar(0,0,255) );

                        d = vertex.point1.x - vertex1.point1.x;  //计算视差
                        x = vertex.point1.x;  //获取左图像标志点的横坐标
                        y = vertex.point1.y;  //获取左图像标志点的纵坐标
                        X1 = -((x-Cx)*Tx/d);  //计算空间横坐标
                        Y1 = -((y-Cy)*Tx/d);  //计算空间纵坐标
                        Z1 = -(f*Tx/d);  //计算空间竖坐标

                        cvPutText( copy ,&quot;Space Coordinate:&quot; , cvPoint(40,200) , &font , cvScalar(0,255,0) );

                        sprintf_s (object_point1,&quot;<%f  %f  %f>&quot;,X1 , Y1, Z1);  //在彩色图像左上角显示标志点空间坐标
                        cvPutText( copy , object_point1 , cvPoint(40,240) , &font , cvScalar(0,255,0) );

                        d = vertex.point2.x - vertex1.point2.x;;
                        x = vertex.point2.x;
                        y = vertex.point2.y;
                        X2 = -((x-Cx)*Tx/d);
                        Y2 = -((y-Cy)*Tx/d);
                        Z2 = -(f*Tx/d);
                        sprintf_s (object_point2,&quot;<%f  %f  %f>&quot;,X2 , Y2, Z2);  //在彩色图像左上角显示标志点空间坐标
                        cvPutText( copy , object_point2 , cvPoint(40,280) , &font , cvScalar(0,255,0) );

                        d = vertex.point3.x - vertex1.point3.x;;
                        x = vertex.point3.x;
                        y = vertex.point3.y;
                        X3 = -((x-Cx)*Tx/d);
                        Y3 = -((y-Cy)*Tx/d);
                        Z3 = -(f*Tx/d);
                        sprintf_s (object_point3,&quot;<%f  %f  %f>&quot;,X3 , Y3, Z3);  //在彩色图像左上角显示标志点空间坐标
                        cvPutText( copy , object_point3 , cvPoint(40,320) , &font , cvScalar(0,255,0) );

                        n1x = X1 - X2;  //以下为计算两个不共线的向量
                        n1y = Y1 - Y2;
                        n1z = Z1 - Z2;

                        n2x = X2 - X3;
                        n2y = Y2 - Y3;
                        n2z = Z2 - Z3;

                        ve_x = (n1y*n2z - n2y*n1z) / (n1x*n2y - n2x*n1y);  //以下计算法向量
                        ve_y = (n1x*n2z - n2x*n1z) / (n2x*n1y - n1x*n2y);
                        ve_z = 1;

                        angle_fw = atan2 (ve_x, ve_z);
                        angle_fw = angle_fw *180 / 3.141592653;
                        angle_fy = atan2 (ve_y, ve_z);
                        angle_fy = -(angle_fy *180 / 3.141592653);

                        sprintf_s (angle_FW,&quot;Azimuth Angle: %f&quot;,angle_fw);  
                        cvPutText( copy , angle_FW , cvPoint(40,360) , &font , cvScalar(255,0,0) );
                        sprintf_s (angle_FY,&quotitch    Angle: %f&quot;,angle_fy);  
                        cvPutText( copy , angle_FY , cvPoint(40,400) , &font , cvScalar(255,0,0) );

                        printf(&quot;%f   %f   %f\\n&quot;, ve_x, ve_y, ve_z);
                        printf(&quot;%f   %f\\n\\n&quot;, angle_fy, angle_fw);

                }       
//                printf(&quot;%d\\n&quot;,is_LRmatched);
//                printf(&quot;%f\\n&quot;, Tx);

                cvSetImageROI(images_combined, cvRect(0, 0, 1024, 768));
                cvCopy (copy, images_combined);
                cvResetImageROI (images_combined);
                cvSetImageROI(images_combined, cvRect(1024, 0, 1024, 768));
                cvCopy (copy1, images_combined);
                cvResetImageROI (images_combined);

                cvLine( images_combined , cvPoint(vertex.point1.x,vertex.point1.y) , cvPoint(vertex1.point1.x + 1024, vertex1.point1.y) , cvScalar(16,199,238) , 2 , 8 );
                cvLine( images_combined , cvPoint(vertex.point2.x,vertex.point2.y) , cvPoint(vertex1.point2.x + 1024, vertex1.point2.y) , cvScalar(16,199,238) , 2 , 8 );
                cvLine( images_combined , cvPoint(vertex.point3.x,vertex.point3.y) , cvPoint(vertex1.point3.x + 1024, vertex1.point3.y) , cvScalar(16,199,238) , 2 , 8 );

                cvShowImage (&quot;合并图像窗口&quot;, images_combined);
                                       
                  cvShowImage( &quot;轮廓图像&quot;, g_gray );  //在窗口中显示这个画布
                cvShowImage( &quot;轮廓图像1&quot;, g_gray1 );  //在窗口中显示这个画布


                if(cvWaitKey(1)==\'c\'){
                        break;
                }
        }

        //这是个Kalman滤波器啊Kalman滤波器O(∩_∩)O~
        CvKalman * kalman = 0;  //定义Kalman滤波器变量
        CvKalman * kalman1 = 0;  //定义Kalman滤波器变量
        kalman = initKalman ( kalman );  //初始化Kalman滤波器
        kalman1 = initKalman ( kalman1 );  //初始化Kalman滤波器

        CvMat * xK = cvCreateMat(8,1,CV_32FC1);//初始化状态变量，依次为重心XY坐标及变化速度，长度及变化速度，面积及变化速度
        CvMat * xK1 = cvCreateMat(8,1,CV_32FC1);//初始化状态变量，依次为重心XY坐标及变化速度，长度及变化速度，面积及变化速度

        xK->data.fl[0]=heart_X;  //X k|k-1 的第一行第一列的元素，即重心的X坐标
        xK->data.fl[1]=heart_Y;  //重心的Y坐标
        /*下面定义初始XY方向速度为0是可以的。第一次赋值其实关系不大，循环几次模型稳定了速度自然准确了！*/
        xK->data.fl[2]=0;  //初始化重心的X方向移动速度为0
        xK->data.fl[3]=0;  //初始化重心的Y方向移动速度为0
        xK->data.fl[4]=lenght_triangle;  //轮廓长度
        xK->data.fl[5]=0;
        xK->data.fl[6]=area_triangle;  //轮廓面积
        xK->data.fl[7]=0;

        xK1->data.fl[0]=heart_X1;  //X k|k-1 的第一行第一列的元素，即重心的X坐标
        xK1->data.fl[1]=heart_Y1;  //重心的Y坐标
        /*下面定义初始XY方向速度为0是可以的。第一次赋值其实关系不大，循环几次模型稳定了速度自然准确了！*/
        xK1->data.fl[2]=0;  //初始化重心的X方向移动速度为0
        xK1->data.fl[3]=0;  //初始化重心的Y方向移动速度为0
        xK1->data.fl[4]=lenght_triangle1;  //轮廓长度
        xK1->data.fl[5]=0;
        xK1->data.fl[6]=area_triangle1;  //轮廓面积
        xK1->data.fl[7]=0;

        CvMat * zK = cvCreateMat(4,1,CV_32FC1);//测量矩阵4维，重心x、y坐标、轮廓长度、轮廓面积，zK=H*xK+vK中的zK
        cvZero(zK);  

        CvMat * zK1 = cvCreateMat(4,1,CV_32FC1);//测量矩阵4维，重心x、y坐标、轮廓长度、轮廓面积，zK=H*xK+vK中的zK
        cvZero(zK1);

        CvMat * vK=cvCreateMat(4,1,CV_32FC1);//测量噪声zK=H*xK+vK中的vK
        cvZero(vK);  //这里设置为0，说明我能精确测出三角形的重心坐标！

        CvMat * vK1=cvCreateMat(4,1,CV_32FC1);//测量噪声zK=H*xK+vK中的vK
        cvZero(vK1);  //这里设置为0，说明我能精确测出三角形的重心坐标！

        CvPoint lastPoint = cvPoint(cvRound(heart_X),cvRound(heart_Y));  //保存上一帧的重心坐标
        CvPoint lastPoint1 = cvPoint(cvRound(heart_X1),cvRound(heart_Y1));  //保存上一帧的重心坐标
        int lastLenght = lenght_triangle;  //保存上一帧的轮廓长度
        int lastArea = area_triangle;  //保存上一帧的轮廓面积
        int lastLenght1 = lenght_triangle1;  //保存上一帧的轮廓长度
        int lastArea1 = area_triangle1;  //保存上一帧的轮廓面积

//        int circle_times = 0;  //Kalman滤波器预热的次数，这里设置为100次，这之后就比较准确了
//        int lost_times = 0;  //保存连续丢失轮廓的次数，当多于一定的次数时，这里定义为50，就重置Kalman滤波器到初始状态

        while (true){
               
                frame=camera.QueryFrame();
                frame1=camera1.QueryFrame();

                if (frame && frame1){
                        cvRemap (frame, image, mx1, my1);
                        cvRemap (frame1, image1, mx2, my2);
                }

                const CvMat * yK = cvKalmanPredict(kalman,0);  //根据X k|k-1 = F * X k-1，计算下一帧重心的预测位置！这里yK就是X k|k-1。
                cvMatMulAdd( kalman->measurement_matrix, xK, vK, zK );//zK=H*xK+vK，此处vK是0，即我能精确测出三角形的重心坐标，zK=H*xK！
                const CvMat * yK1 = cvKalmanPredict(kalman1,0);  //根据X k|k-1 = F * X k-1，计算下一帧重心的预测位置！这里yK就是X k|k-1。
                cvMatMulAdd( kalman1->measurement_matrix, xK1, vK1, zK1 );//zK=H*xK+vK，此处vK是0，即我能精确测出三角形的重心坐标，zK=H*xK！

                cvCopy(image,copy);   //复制每帧原始图像，前期调试阶段使用，后期可考虑删除相关代码！O(∩_∩)O~
                cvCopy(image1,copy1);   //复制每帧原始图像，前期调试阶段使用，后期可考虑删除相关代码！O(∩_∩)O~

                if( g_storage==NULL ) {  //
                        g_gray = cvCreateImage( cvGetSize(image), 8, 1 );
                           g_storage = cvCreateMemStorage(0);
                  } else {
                    cvClearMemStorage( g_storage );
                }

                if( g_storage1==NULL ) {  //
                        g_gray1 = cvCreateImage( cvGetSize(image1), 8, 1 );
                           g_storage1 = cvCreateMemStorage(0);
                  } else {
                    cvClearMemStorage( g_storage1 );
                }

                 
                cvCvtColor( image, g_gray, CV_BGR2GRAY );  //将原始彩色图像转换为灰度图像
                cvCvtColor( image1, g_gray1, CV_BGR2GRAY );  //将原始彩色图像转换为灰度图像

                cvThreshold( g_gray, g_gray, g_thresh, 255, CV_THRESH_BINARY );  //根据拖动滚动条，对灰度图像进行阈值化
                cvThreshold( g_gray1, g_gray1, g_thresh1, 255, CV_THRESH_BINARY );  //根据拖动滚动条，对灰度图像进行阈值化

                cvShowImage(&quot;阈值化图像&quot;,g_gray);
                cvShowImage(&quot;阈值化图像1&quot;,g_gray1);

                scanner = cvStartFindContours(g_gray,g_storage,sizeof(CvContour),CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE,cvPoint(0,0));
                scanner1 = cvStartFindContours(g_gray1,g_storage1,sizeof(CvContour),CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE,cvPoint(0,0));

                is_getVertexAtLeft = 0;
                is_getVertexAtRight = 0;

                is_LRmatched = 0;


                /*************以下while遍历左图像每个轮廓，分离出三角形的轮廓！*************/
                numberOfContour=0;  //记录每帧一共截取出来几个轮廓（正常情况应该就出现1个轮廓！）
                                                        //如果发现下面的循环之后numberOfContour大于1，好吧，我们发现了冒牌轮廓 ~~~~(>_<)~~~~
                contour_triangle = 0;  //每次循环之前，要清理上次的垃圾信息！

                while (contours = cvFindNextContour(scanner)){
                       
                        tmp_lenght = fabs(cvContourPerimeter(contours));  //计算一个轮廓的长度
                        if (tmp_lenght < min_lenght || tmp_lenght > max_lenght){
                                cvSubstituteContour(scanner,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }
                       
                        tmp_area = fabs(cvContourArea(contours));  //计算一个轮廓的长度       
                        if (tmp_area < min_area || tmp_area > max_area){
                                cvSubstituteContour(scanner,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }

                        numberOfContour++;  //发现一个疑似三角形，就自加1
                       
                        contour_triangle = contours;  //将“准三角形轮廓”拷贝，退出这个循环后我再慢慢拷问它是不是真的 ╭(╯^╰)╮

                        lenght_triangle = tmp_lenght;  //下面两个值要保存下来，当切入Kalman循环时这两个值会作为状态变量的初始值！

                        area_triangle = tmp_area;

                }
               

                //下面这个if-else就是要严刑拷问“准三角形轮廓”的真身的！
                /*如果没得到“准三角形轮廓”（系统刚启动时阈值化调得不理想或者后期的的确确没找到像三角形的轮廓），那我们就没必要在图像上和控制台上显示
                坐标了！如果contour_triangle不为空但是numberOfContour大于1，说明有假冒的“三角形轮廓”混进来了，既然它混进来了，那我就也不显示坐标了！*/
                if ( contour_triangle &&  numberOfContour == 1 ){  

                        vertex = getTriangleVertex( contour_triangle );  //获取三角形轮廓的顶点（标志点）

                        if ( vertex.usefull != 0 ){  //判断是否利用分离出来的三角形成功获取到了标志点
                                heart_X = ( vertex.point1.x + vertex.point2.x + vertex.point3.x )/3;  //计算重心坐标
                                heart_Y = ( vertex.point1.y + vertex.point2.y + vertex.point3.y )/3;
                                cvLine( copy , cvPoint(vertex.point1.x,vertex.point1.y) , cvPoint(heart_X,heart_Y) , cvScalar(0,255,0) , 2 , 8 );  //绘制三条中线
                                cvLine( copy , cvPoint(vertex.point2.x,vertex.point2.y) , cvPoint(heart_X,heart_Y) , cvScalar(0,255,0) , 2 , 8 );
                                cvLine( copy , cvPoint(vertex.point3.x,vertex.point3.y) , cvPoint(heart_X,heart_Y) , cvScalar(0,255,0) , 2 , 8 );
                                cvCircle( copy , cvPoint(vertex.point1.x,vertex.point1.y) , 1 , cvScalar(0,0,255) , 5 , 8 );  //在彩色图像上标记标志点
                                cvCircle( copy , cvPoint(vertex.point2.x,vertex.point2.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy , cvPoint(vertex.point3.x,vertex.point3.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy , cvPoint(heart_X,heart_Y) , 1 , cvScalar(0,0,255) , 5 , 8 );

                                is_getVertexAtLeft = 1;  //左图像获取到了三角形标志点                               
/*
                                sprintf_s (point1,&quot;<%d   %d>&quot;,vertex.point1.x , vertex.point1.y);  //在彩色图像左上角显示标志点坐标
                                cvPutText( copy , point1 , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                sprintf_s (point2,&quot;<%d   %d>&quot;,vertex.point2.x , vertex.point2.y);
                                cvPutText( copy , point2 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                                sprintf_s (point3,&quot;<%d   %d>&quot;,vertex.point3.x , vertex.point3.y);
                                cvPutText( copy , point3 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
*/
                                cvPutText( copy , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );

                        }else{
                                is_getVertexAtLeft = 0;  //左图像未获取到三角形标志点
                                cvPutText( copy , &quot;Fail to get vertex!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );
                        }
                }else{
                        if( !contour_triangle ){  //此时是米有找到符合条件的轮廓
                                is_getVertexAtLeft = 0;  //左图像未获取到三角形标志点
                                cvPutText( copy , &quot;Fail to get contour of the triangle!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );
                        }else if( numberOfContour > 1 ){  //此时是找到了多于1个三角形的轮廓
                                is_getVertexAtLeft = 0;  //左图像未获取到三角形标志点
                                cvDrawContours( copy ,contour_triangle ,cvScalar(255,255,0),cvScalar(255,255,0),2,3,8,cvPoint(0,0));
                                cvPutText( copy , &quot;Find one more like-trianglecontour!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );
                        }
                }


                contours = cvEndFindContours(&scanner);

                cvZero( g_gray );  //将阈值化图像清空，也就是一个黑屏，当作下面的画布
               
                if( contours){  //如果有轮廓信息，就在画布上绘制出轮廓   
                        cvDrawContours(g_gray,contours,cvScalarAll(255),cvScalarAll(255),2,1,8,cvPoint(0,0));
                }

//                if (is_getVertexAtLeft == 0){
//                        continue;
//                }


                /////////////////////////////////////////////////////////////////////////////////////////////
                /////////////////////////////////////////////////////////////////////////////////////////////
                /*************以下while遍历右图像每个轮廓，分离出三角形的轮廓！*************/
                numberOfContour1=0;  //记录每帧一共截取出来几个轮廓（正常情况应该就出现1个轮廓！）
                                                        //如果发现下面的循环之后numberOfContour大于1，好吧，我们发现了冒牌轮廓 ~~~~(>_<)~~~~
                contour_triangle1 = 0;  //每次循环之前，要清理上次的垃圾信息！

                while (contours1 = cvFindNextContour(scanner1)){
                       
                        tmp_lenght1 = fabs(cvContourPerimeter(contours1));  //计算一个轮廓的长度
                        if (tmp_lenght1 < min_lenght || tmp_lenght1 > max_lenght){
                                cvSubstituteContour(scanner1,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }
                       
                        tmp_area1 = fabs(cvContourArea(contours1));  //计算一个轮廓的长度       
                        if (tmp_area1 < min_area || tmp_area1 > max_area){
                                cvSubstituteContour(scanner1,NULL);//删除当前的轮廓
                                continue;  //接着处理下个轮廓
                        }

                        numberOfContour1++;  //发现一个疑似三角形，就自加1
                       
                        contour_triangle1 = contours1;  //将“准三角形轮廓”拷贝，退出这个循环后我再慢慢拷问它是不是真的 ╭(╯^╰)╮

                        lenght_triangle1 = tmp_lenght1;  //下面两个值要保存下来，当切入Kalman循环时这两个值会作为状态变量的初始值！

                        area_triangle1 = tmp_area1;

                }
               

                //下面这个if-else就是要严刑拷问“准三角形轮廓”的真身的！
                /*如果没得到“准三角形轮廓”（系统刚启动时阈值化调得不理想或者后期的的确确没找到像三角形的轮廓），那我们就没必要在图像上和控制台上显示
                坐标了！如果contour_triangle不为空但是numberOfContour大于1，说明有假冒的“三角形轮廓”混进来了，既然它混进来了，那我就也不显示坐标了！*/
                if ( contour_triangle1 &&  numberOfContour1 == 1 ){  

                        vertex1 = getTriangleVertex( contour_triangle1 );  //获取三角形轮廓的顶点（标志点）

                        if ( vertex1.usefull != 0 ){  //判断是否利用分离出来的三角形成功获取到了标志点
                                heart_X1 = ( vertex1.point1.x + vertex1.point2.x + vertex1.point3.x )/3;  //计算重心坐标
                                heart_Y1 = ( vertex1.point1.y + vertex1.point2.y + vertex1.point3.y )/3;
                                cvLine( copy1 , cvPoint(vertex1.point1.x,vertex1.point1.y) , cvPoint(heart_X1,heart_Y1) , cvScalar(0,255,0) , 2 , 8 );  //绘制三条中线
                                cvLine( copy1 , cvPoint(vertex1.point2.x,vertex1.point2.y) , cvPoint(heart_X1,heart_Y1) , cvScalar(0,255,0) , 2 , 8 );
                                cvLine( copy1 , cvPoint(vertex1.point3.x,vertex1.point3.y) , cvPoint(heart_X1,heart_Y1) , cvScalar(0,255,0) , 2 , 8 );
                                cvCircle( copy1 , cvPoint(vertex1.point1.x,vertex1.point1.y) , 1 , cvScalar(0,0,255) , 5 , 8 );  //在彩色图像上标记标志点
                                cvCircle( copy1 , cvPoint(vertex1.point2.x,vertex1.point2.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy1 , cvPoint(vertex1.point3.x,vertex1.point3.y) , 1 , cvScalar(0,0,255) , 5 , 8 );
                                cvCircle( copy1 , cvPoint(heart_X1,heart_Y1) , 1 , cvScalar(0,0,255) , 5 , 8 );

                                is_getVertexAtRight = 1;  //右图像获取到了三角形标志点
                               
/*
                                sprintf_s (point11,&quot;<%d   %d>&quot;,vertex1.point1.x , vertex1.point1.y);  //在彩色图像左上角显示标志点坐标
                                cvPutText( copy1 , point11 , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                sprintf_s (point21,&quot;<%d   %d>&quot;,vertex1.point2.x , vertex1.point2.y);
                                cvPutText( copy1 , point21 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                                sprintf_s (point31,&quot;<%d   %d>&quot;,vertex1.point3.x , vertex1.point3.y);
                                cvPutText( copy1 , point31 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
*/
                                cvPutText( copy1 , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );

                        }else{
                                is_getVertexAtRight = 0;  //右图像未获取到三角形标志点
                                cvPutText( copy1 , &quot;Fail to get vertex!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy1 , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );
                        }

                }else{
                        if( !contour_triangle1 ){  //此时是米有找到符合条件的轮廓
                                is_getVertexAtRight = 0;  //右图像未获取到三角形标志点
                                cvPutText( copy1 , &quot;Fail to get contour of the triangle!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy1 , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );
                        }else if( numberOfContour1 > 1 ){  //此时是找到了多于1个三角形的轮廓
                                is_getVertexAtRight = 0;  //右图像未获取到三角形标志点
                                cvDrawContours( copy1 ,contour_triangle1 ,cvScalar(255,255,0),cvScalar(255,255,0),2,3,8,cvPoint(0,0));
                                cvPutText( copy1 , &quot;Find one more like-trianglecontour!!&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                                cvPutText( copy1 , &quot;Kalman is powered on&quot; , cvPoint(600,40) , &font , cvScalar(0,255,0) );
                        }
                }


                contours1 = cvEndFindContours(&scanner1);

                cvZero( g_gray1 );  //将阈值化图像清空，也就是一个黑屏，当作下面的画布
               
                if( contours1){  //如果有轮廓信息，就在画布上绘制出轮廓   
                        cvDrawContours(g_gray1,contours1,cvScalarAll(255),cvScalarAll(255),2,1,8,cvPoint(0,0));
                }
                /////////////////////////////////////////////////////////////////////////////////////////////
                /////////////////////////////////////////////////////////////////////////////////////////////
                 

                if (is_getVertexAtLeft == 1 && is_getVertexAtRight == 1){

                        int x;  //左目一个标志点的横坐标
                        int y;  //左目一个标志点的纵坐标
                        double X1, Y1, Z1;  //第一个标志点的空间三维坐标
                        double X2, Y2, Z2;  //第二个标志点的空间三维坐标
                        double X3, Y3, Z3;  //第三个标志点的空间三维坐标

                        double n1x, n1y, n1z;  //第一个向量
                        double n2x, n2y, n2z;  //第二个向量

                        double ve_x, ve_y, ve_z;  //三角形所在平面的空间法向量

                        double angle_fy, angle_fw;  //俯仰角、方位角
                       
                        //cvZero (images_combined);

                        is_LRmatched = sortVertexs( & vertex, & vertex1);  //左右目标志点匹配函数

                        cvPutText( copy , &quot;Image Coordinate:&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                        sprintf_s (point1,&quot;<%d   %d>&quot;,vertex.point1.x , vertex.point1.y);  //在彩色图像左上角显示标志点图象坐标
                        cvPutText( copy , point1 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                        sprintf_s (point2,&quot;<%d   %d>&quot;,vertex.point2.x , vertex.point2.y);
                        cvPutText( copy , point2 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
                        sprintf_s (point3,&quot;<%d   %d>&quot;,vertex.point3.x , vertex.point3.y);
                        cvPutText( copy , point3 , cvPoint(40,160) , &font , cvScalar(0,0,255) );

                        cvPutText( copy1 , &quot;Image Coordinate:&quot; , cvPoint(40,40) , &font , cvScalar(0,0,255) );
                        sprintf_s (point11,&quot;<%d   %d>&quot;,vertex1.point1.x , vertex1.point1.y);  //在彩色图像左上角显示标志点图象坐标
                        cvPutText( copy1 , point11 , cvPoint(40,80) , &font , cvScalar(0,0,255) );
                        sprintf_s (point21,&quot;<%d   %d>&quot;,vertex1.point2.x , vertex1.point2.y);
                        cvPutText( copy1 , point21 , cvPoint(40,120) , &font , cvScalar(0,0,255) );
                        sprintf_s (point31,&quot;<%d   %d>&quot;,vertex1.point3.x , vertex1.point3.y);
                        cvPutText( copy1 , point31 , cvPoint(40,160) , &font , cvScalar(0,0,255) );

                        d = vertex.point1.x - vertex1.point1.x;  //计算视差
                        x = vertex.point1.x;  //获取左图像标志点的横坐标
                        y = vertex.point1.y;  //获取左图像标志点的纵坐标
                        X1 = -((x-Cx)*Tx/d);  //计算空间横坐标
                        Y1 = -((y-Cy)*Tx/d);  //计算空间纵坐标
                        Z1 = -(f*Tx/d);  //计算空间竖坐标

                        cvPutText( copy ,&quot;Space Coordinate:&quot; , cvPoint(40,200) , &font , cvScalar(0,255,0) );

                        sprintf_s (object_point1,&quot;<%f  %f  %f>&quot;,X1 , Y1, Z1);  //在彩色图像左上角显示标志点空间坐标
                        cvPutText( copy , object_point1 , cvPoint(40,240) , &font , cvScalar(0,255,0) );

                        d = vertex.point2.x - vertex1.point2.x;
                        x = vertex.point2.x;
                        y = vertex.point2.y;
                        X2 = -((x-Cx)*Tx/d);
                        Y2 = -((y-Cy)*Tx/d);
                        Z2 = -(f*Tx/d);
                        sprintf_s (object_point2,&quot;<%f  %f  %f>&quot;,X2 , Y2, Z2);  //在彩色图像左上角显示标志点空间坐标
                        cvPutText( copy , object_point2 , cvPoint(40,280) , &font , cvScalar(0,255,0) );

                        d = vertex.point3.x - vertex1.point3.x;
                        x = vertex.point3.x;
                        y = vertex.point3.y;
                        X3 = -((x-Cx)*Tx/d);
                        Y3 = -((y-Cy)*Tx/d);
                        Z3 = -(f*Tx/d);
                        sprintf_s (object_point3,&quot;<%f  %f  %f>&quot;,X3 , Y3, Z3);  //在彩色图像左上角显示标志点空间坐标
                        cvPutText( copy , object_point3 , cvPoint(40,320) , &font , cvScalar(0,255,0) );

                        n1x = X1 - X2;  //以下为计算两个不共线的向量
                        n1y = Y1 - Y2;
                        n1z = Z1 - Z2;

                        n2x = X2 - X3;
                        n2y = Y2 - Y3;
                        n2z = Z2 - Z3;

                        ve_x = (n1y*n2z - n2y*n1z) / (n1x*n2y - n2x*n1y);  //以下计算法向量
                        ve_y = (n1x*n2z - n2x*n1z) / (n2x*n1y - n1x*n2y);
                        ve_z = 1;

                        angle_fw = atan2 (ve_x, ve_z);
                        angle_fw = angle_fw *180 / 3.141592653;
                        angle_fy = atan2 (ve_y, ve_z);
                        angle_fy = -(angle_fy *180 / 3.141592653);

                        sprintf_s (angle_FW,&quot;Azimuth Angle: %f&quot;,angle_fw);  
                        cvPutText( copy , angle_FW , cvPoint(40,360) , &font , cvScalar(255,0,0) );
                        sprintf_s (angle_FY,&quotitch    Angle: %f&quot;,angle_fy);  
                        cvPutText( copy , angle_FY , cvPoint(40,400) , &font , cvScalar(255,0,0) );

                        printf(&quot;%f   %f   %f\\n&quot;, ve_x, ve_y, ve_z);
                        printf(&quot;%f   %f\\n\\n&quot;, angle_fy, angle_fw);

                }       
//                printf(&quot;%d\\n&quot;,is_LRmatched);
//                printf(&quot;%f\\n&quot;, Tx);

                cvSetImageROI(images_combined, cvRect(0, 0, 1024, 768));
                cvCopy (copy, images_combined);
                cvResetImageROI (images_combined);
                cvSetImageROI(images_combined, cvRect(1024, 0, 1024, 768));
                cvCopy (copy1, images_combined);
                cvResetImageROI (images_combined);

                cvLine( images_combined , cvPoint(vertex.point1.x,vertex.point1.y) , cvPoint(vertex1.point1.x + 1024, vertex1.point1.y) , cvScalar(16,199,238) , 2 , 8 );
                cvLine( images_combined , cvPoint(vertex.point2.x,vertex.point2.y) , cvPoint(vertex1.point2.x + 1024, vertex1.point2.y) , cvScalar(16,199,238) , 2 , 8 );
                cvLine( images_combined , cvPoint(vertex.point3.x,vertex.point3.y) , cvPoint(vertex1.point3.x + 1024, vertex1.point3.y) , cvScalar(16,199,238) , 2 , 8 );

                cvShowImage (&quot;合并图像窗口&quot;, images_combined);
                                       
                  cvShowImage( &quot;轮廓图像&quot;, g_gray );  //在窗口中显示这个画布
                cvShowImage( &quot;轮廓图像1&quot;, g_gray1 );  //在窗口中显示这个画布

                xK->data.fl[0]=heart_X;  //每次循环后，要设置X k-1为本次重心的XY坐标
                xK->data.fl[1]=heart_Y;
                /*下面这两句困扰了我4天，其实想通了是非常简单的！！有米有。*/
                xK->data.fl[2]=heart_X - lastPoint.x;  //速度我分别取本帧与上一帧重心坐标X、Y的差值！精髓了这里！(*^__^*)
                xK->data.fl[3]=heart_Y - lastPoint.y;
                xK->data.fl[4]=lenght_triangle;  //设置状态变量中的长度
                xK->data.fl[5]=lenght_triangle - lastLenght;  //长度变化速度
                xK->data.fl[6]=area_triangle;
                xK->data.fl[7]=area_triangle - lastArea;
                lastPoint.x = heart_X;  //以下4句是将本次的重心坐标、长度和面积数值保存，作为下次循环的旧值
                lastPoint.y = heart_Y;
                lastLenght = lenght_triangle;
                lastArea = area_triangle;
                cvKalmanCorrect( kalman, zK );  //这次循环结束了，我要更新我的可爱的Kalman滤波器的模型了！一切努力就是为了下个循环更精准！！

                xK1->data.fl[0]=heart_X1;  //每次循环后，要设置X k-1为本次重心的XY坐标
                xK1->data.fl[1]=heart_Y1;
                /*下面这两句困扰了我4天，其实想通了是非常简单的！！有米有。*/
                xK1->data.fl[2]=heart_X1 - lastPoint1.x;  //速度我分别取本帧与上一帧重心坐标X、Y的差值！精髓了这里！(*^__^*)
                xK1->data.fl[3]=heart_Y1 - lastPoint1.y;
                xK1->data.fl[4]=lenght_triangle1;  //设置状态变量中的长度
                xK1->data.fl[5]=lenght_triangle1 - lastLenght1;  //长度变化速度
                xK1->data.fl[6]=area_triangle1;
                xK1->data.fl[7]=area_triangle1 - lastArea1;
                lastPoint1.x = heart_X1;  //以下4句是将本次的重心坐标、长度和面积数值保存，作为下次循环的旧值
                lastPoint1.y = heart_Y1;
                lastLenght1 = lenght_triangle1;
                lastArea1 = area_triangle1;
                cvKalmanCorrect( kalman1, zK1 );  //这次循环结束了，我要更新我的可爱的Kalman滤波器的模型了！一切努力就是为了下个循环更精准！！


                if(cvWaitKey(1)==\'c\'){
                        break;
                }
        }

        camera.CloseCamera(); //关闭摄像头
        camera1.CloseCamera(); //关闭摄像头
        cvReleaseImage(&frame);  //释放原始彩色图像
        cvReleaseImage(&image);  //释放原始彩色图像
        cvReleaseImage(&g_gray);  //释放灰度图像
        cvReleaseImage(&copy);
        cvReleaseImage(&frame1);  //释放原始彩色图像
        cvReleaseImage(&image1);  //释放原始彩色图像
        cvReleaseImage(&g_gray1);  //释放灰度图像
        cvReleaseImage(&copy1);
        cvDestroyWindow(&quot;合并图像窗口&quot;);  //销毁窗口
        cvDestroyWindow(&quot;轮廓图像&quot;);  //销毁窗口
        cvDestroyWindow(&quot;阈值化图像&quot;);
        cvDestroyWindow(&quot;轮廓图像1&quot;);  //销毁窗口
        cvDestroyWindow(&quot;阈值化图像1&quot;);


        return 0;
}

如果有什么问题的话，欢迎大家在本帖留言！

longway

应该是while(a < number_image_copy) 吧？加了=号会越界吧~

pyemma

楼主有没有单目摄像机标定的那个算法实现啊，看了张正友的那个论文了，对他的原理有了大概的了解，但是想自己再实现优化一下无从下手

pyemma

[quote=&quot;peterli&quot;:374csnf9]立体标定是什么意思

就是双目摄像机标定   主要计算两个镜头之间的R和t矩阵[/quote:374csnf9]
分成两次标定两个摄像机然后通过他们的外参数矩阵不能直接得到两个摄像机的位置关系吗？

guvcolie

应该是while(a < number_image_copy) 吧？加了=号会越界吧~

不是的！这两个变量都是从1开始赋值的！

liguangqi133

老师我想问下，我主要目的是双目测量一个物体的两点的距离或者两条直线的角度，对测距无要求，所以我想实现两幅图点的匹配，最后在得到匹配点的三维坐标。先是两个相机的立体标定我求出了内外参数，以及基础矩阵等等，接下来我是canny目标图的边缘再提取角点并亚像素精度，然后呢？是用提取的角点再求基本矩阵还是直接用之前的求得的基础矩阵计算cvComputeCorrespondEpilines极线，极线算出来了我又怎么办，我不需要图像匹配，我只需要通过极线实现点的匹配，最后得到点的三维信息，可是我不知道怎么用opencv怎么编极线上点的匹配，没看到相关函数，老师请指点，有程序当然是最好，没有也请把思路给我讲下

mengyun29

感谢LZ

echo77

卤煮 我用SIFT实现两幅图像的匹配 图像首先是校正过了的 然后根据匹配点的视差d和立体标定的f Tx测距 但是距离不准 总是偏大很多
匹配点明显是水平对准的 因为经过校正后图像行对准 应该是哪里的误差呢
我用opencv的立体标定 拍过几组棋盘格进行试验 每一次的结果都不一样 特别是主点位置 在280~350之间不定 应该怎么确定那次标定的正确呢
PS 我的棋盘角点数设置为7x6 方格大小30mm 这些参数对标定结果有没有影响呢

guvcolie

卤煮 我用SIFT实现两幅图像的匹配 图像首先是校正过了的 然后根据匹配点的视差d和立体标定的f Tx测距 但是距离不准 总是偏大很多
匹配点明显是水平对准的 因为经过校正后图像行对准 应该是哪里的误差呢
我用opencv的立体标定 拍过几组棋盘格进行试验 每一次的结果都不一样 特别是主点位置 在280~350之间不定 应该怎么确定那次标定的正确呢
PS 我的棋盘角点数设置为7x6 方格大小30mm 这些参数对标定结果有没有影响呢

你好！应该是你的f 和Tx有误差，我建议你每次拍摄20+对图像对进行标定，而且标定时标定板要处于各个位置，最好不要重复，而且最好保证每对都要能标定处理。
对于标定结果的精确性，我建议你再将你用OpenCV标定的图像对再用matlab标定箱重新标定一遍，相对来说还是matlab的标定箱比较精确，但是opencv如果标定的图像对比较好的话，精度也是很好的，和matlab的标定精度没有什么区别！