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会一直叠加
}