public void Bootstrap(Mat img)
        {
            ValidateImages(null, img);

            _bootstrapKp.Clear();
            _detector.DetectRaw(img, _bootstrapKp);

            _trackedFeatures = new VectorOfKeyPoint(_bootstrapKp.ToArray());

            _trackedFeatures3D.Clear();

            CvInvoke.CvtColor(img, _prevGray, ColorConversion.Bgr2Gray);
        }
 private void ObjectPointsCal(object state)
 {
     if (BM_CAL_FLAG)                                //BM视差图计算
     {
         //获取原图
         Data.leftImg.CopyTo(bm_lsrc);
         Data.rightImg.CopyTo(bm_rsrc);
         //转换为灰度图
         CvInvoke.CvtColor(bm_lsrc, bm_lsrc, ColorConversion.Bgr2Gray);
         CvInvoke.CvtColor(bm_rsrc, bm_rsrc, ColorConversion.Bgr2Gray);
         bm.Compute(bm_lsrc, bm_rsrc, bm_distImg);                                              //BM算法只能处理灰度图
         bm_distImg.ConvertTo(bm_distImg, DepthType.Cv32F, 1.0 / 16);                           //除16得到真正的视差图 -----这里的数据需要再次使用
         bm_distImg8U = new Mat(bm_distImg.Size, DepthType.Cv8U, 1);
         CvInvoke.Normalize(bm_distImg, bm_distImg8U, 0, 255, NormType.MinMax, DepthType.Cv8U); //归一化后显示
         this.imageBox1.Image = bm_distImg8U;
     }
     if (SHOWCONTOURS_FLAG)                         //轮廓角点计算
     {
         contourImg.SetTo(new MCvScalar(0, 0, 0));
         Data.leftImg.CopyTo(contourSrc);
         CvInvoke.CvtColor(contourSrc, grayImg, ColorConversion.Bgr2Gray);
         CvInvoke.Canny(grayImg, cannyImg, 100, 200, 3, false);                            //转换为二值图
         CvInvoke.FindContours(cannyImg, contours, hierarchy, RetrType.External, ChainApproxMethod.ChainApproxNone);
         CvInvoke.DrawContours(contourImg, contours, -1, new MCvScalar(255, 255, 255), 2); //绘制所有轮廓
         cornerPoints = gFTT.Detect(grayImg);                                              //计算角点
         for (int i = 0; i < cornerPoints.Length; i++)
         {
             Point pt = new Point();
             pt.X = (int)cornerPoints[i].Point.X;
             pt.Y = (int)cornerPoints[i].Point.Y;
             CvInvoke.Circle(contourImg, pt, 3, new MCvScalar(0, 0, 255), -1); //绘制所有角点
         }
         this.imageBox2.Image = contourImg;                                    //显示图像
     }
     if (OBJPOINTS_CAL_FLAG)                                                   //计算角点的三维坐标在TextBox中显示
     {
         //2种方法
         Image <Gray, Single> bm_distImg_C = bm_distImg.ToImage <Gray, Single>();
         try
         {
             for (int i = 0; i < cornerPoints.Length; i++)
             {
                 MCvPoint3D32f[] ptf = new MCvPoint3D32f[1];                          //转换计算得到的角点,记得清空
                 ptf[0].X = cornerPoints[i].Point.X;
                 ptf[0].Y = cornerPoints[i].Point.Y;
                 if (ptf[0].X > bm_distImg_C.Width)                               //数组越界检查
                 {
                     ptf[0].X = bm_distImg_C.Width - 1;
                 }
                 if (ptf[0].Y > bm_distImg_C.Height)
                 {
                     ptf[0].Y = bm_distImg_C.Height - 1;
                 }
                 ptf[0].Z = bm_distImg_C.Data[(int)ptf[0].X, (int)ptf[0].Y, 0];      //获取角点在视差图中的深度值(单位是像素)
                 cornerPoints_vec.Push(ptf);                                         //存储转换好后的值
             }
             CvInvoke.PerspectiveTransform(cornerPoints_vec, objXYZ, Data.Q);        //透视变换,得到稀疏特征点在摄像机坐标系下的坐标
             //方法2
             //CvInvoke.ReprojectImageTo3D(bm_distImg, bm_image3D, Data.Q, true);
             //for(int i = 0; i < cornerPoints.Length; i++)
             //{
             //    MCvPoint3D32f[] ptf = new MCvPoint3D32f[1];
             //    //需要处理数组越界问题
             //    ptf[0].X = this.bm_image3D.Data[(int)cornerPoints[i].Point.X, (int)cornerPoints[i].Point.Y, 0];
             //    ptf[0].Y = this.bm_image3D.Data[(int)cornerPoints[i].Point.X, (int)cornerPoints[i].Point.Y, 1];
             //    ptf[0].Z = this.bm_image3D.Data[(int)cornerPoints[i].Point.X, (int)cornerPoints[i].Point.Y, 2];
             //    objXYZ.Push(ptf);               //存储计算好的空间坐标点
             //}
             //写入数据  必须调用Invoke方法
             this.Invoke(new UpdateTextBox(UpdateTextBoxFunc), new object[] { });
         }
         catch (Exception e)
         {
             Data.LogString = "[error]  " + e.Message;
         }
     }
     cornerPoints_vec.Clear();                        //清空元素,Vector会一直叠加
 }