void viewDepthUndistionMat(CvMat src, CvMat depthMat)
{
if (src == null)
{
return;
}
double pX = this.UndistortionData.CameraStruct.PrincipalX;
double pY = this.UndistortionData.CameraStruct.PrincipalY;
int focalX = (int)(this.UndistortionData.CameraStruct.FocalX / 2);
int focalY = (int)(this.UndistortionData.CameraStruct.FocalY / 2);
if (src.ElemChannels == 3)
{
CvMat quadratic = CvEx.InitCvMat(src, MatrixType.F32C1);
CvMat scale = CvEx.InitCvMat(src, MatrixType.F32C1);
CvMat offset = CvEx.InitCvMat(src, MatrixType.F32C1);
src.Split(quadratic, scale, offset, null);
float maxQuadratic, minQuadratic;
float maxScale, minScale;
float maxOffset, minOffset;
using (CvMat displayMatQuadratic = getDepthUndistortMatImage(out maxQuadratic, out minQuadratic, quadratic, 0f))
using (CvMat displayMatScale = getDepthUndistortMatImage(out maxScale, out minScale, scale, 1f))
using (CvMat displayMatOffset = getDepthUndistortMatImage(out maxOffset, out minOffset, offset, null))
{
using (CvMat black = CvEx.InitCvMat(src, MatrixType.U8C1))
{
CvMat total = CvEx.InitCvMat(src, MatrixType.U8C3);
black.Zero();
total.Merge(displayMatQuadratic, displayMatScale, displayMatOffset, null);
total.PutText(string.Format("RangeQuadatic: {0:0.000} to {1:0.000}", minQuadratic, maxQuadratic), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.PutText(string.Format("RangeScale: {0:0.000} to {1:0.000}", minScale, maxScale), new CvPoint(0, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.PutText(string.Format("RangeOffset: {0:0.000} to {1:0.000}", minOffset, maxOffset), new CvPoint(0, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.DrawLine(new CvPoint((int)pX, 0), new CvPoint((int)pX, total.Rows), new CvScalar(255, 255, 0));
total.DrawLine(new CvPoint(0, (int)pY), new CvPoint(total.Cols, (int)pY), new CvScalar(255, 255, 0));
total.DrawEllipse(new CvPoint((int)pX, (int)pY), new CvSize(focalX, focalY), 0, 0, 360, new CvScalar(255, 0, 0));
putImage(total, PixelFormats.Bgr24);
}
}
}
else if (src.ElemChannels == 2)
{
CvMat scale = CvEx.InitCvMat(src, MatrixType.F32C1);
CvMat offset = CvEx.InitCvMat(src, MatrixType.F32C1);
src.Split(scale, offset, null, null);
float maxScale, minScale;
float maxOffset, minOffset;
using (CvMat displayMatScale = getDepthUndistortMatImage(out maxScale, out minScale, scale, 1f))
using (CvMat displayMatOffset = getDepthUndistortMatImage(out maxOffset, out minOffset, offset, null))
{
using (CvMat black = CvEx.InitCvMat(src, MatrixType.U8C1))
{
CvMat total = CvEx.InitCvMat(src, MatrixType.U8C3);
black.Zero();
total.Merge(black, displayMatScale, displayMatOffset, null);
total.PutText(string.Format("RangeScale: {0:0.000} to {1:0.000}", minScale, maxScale), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.PutText(string.Format("RangeOffset: {0:0.000} to {1:0.000}", minOffset, maxOffset), new CvPoint(0, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.DrawLine(new CvPoint((int)pX, 0), new CvPoint((int)pX, total.Rows), new CvScalar(255, 255, 0));
total.DrawLine(new CvPoint(0, (int)pY), new CvPoint(total.Cols, (int)pY), new CvScalar(255, 255, 0));
total.DrawEllipse(new CvPoint((int)pX, (int)pY), new CvSize(focalX, focalY), 0, 0, 360, new CvScalar(255, 0, 0));
putImage(total, PixelFormats.Bgr24);
}
}
}
else if (src.ElemChannels == 1)
{
float max, min;
using (CvMat displayMat = getDepthUndistortMatImage(out max, out min, src, null))
{
CvMat total = CvEx.InitCvMat(src, MatrixType.U8C3);
total.Merge(displayMat, displayMat, displayMat, null);
total.PutText(string.Format("Range: {0:0.000} to {1:0.000}", min, max), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
if (depthMat != null)
{
double?pDepth = CvEx.Get2DSubPixel(depthMat, new CvPoint2D32f(pX, pY), 0);
if (pDepth.HasValue)
{
total.PutText(pDepth.Value.ToString("0"), new CvPoint((int)pX, (int)pY), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
}
total.DrawLine(new CvPoint((int)pX, 0), new CvPoint((int)pX, total.Rows), new CvScalar(255, 255, 0));
total.DrawLine(new CvPoint(0, (int)pY), new CvPoint(total.Cols, (int)pY), new CvScalar(255, 255, 0));
total.DrawEllipse(new CvPoint((int)pX, (int)pY), new CvSize(focalX, focalY), 0, 0, 360, new CvScalar(255, 0, 0));
}
putImage(total, PixelFormats.Bgr24);
}
}
}
