public static Matrix <double> ToMathNetMatrix(this Emgu.CV.Mat mat, bool isAppend = false)
{
Emgu.CV.Matrix <double> matrix = new Emgu.CV.Matrix <double>(mat.Rows, mat.Cols);
mat.CopyTo(matrix);
double[,] array;
if (isAppend)
{
array = new double[mat.Rows + 1, mat.Cols];
}
else
{
array = new double[mat.Rows, mat.Cols];
}
for (int i = 0; i < mat.Rows; i++)
{
for (int j = 0; j < mat.Cols; j++)
{
array[i, j] = matrix[i, j];
}
}
if (isAppend)
{
for (int j = 0; j < mat.Cols - 1; j++)
{
array[mat.Rows, j] = 0;
}
array[mat.Rows, mat.Cols - 1] = 1;
}
return(DenseMatrix.OfArray(array));
}
private void Calibrate()
{
if (m_skeletonCalibPoints.Count == m_calibPoints.Count)
{
//seketon 3D positions --> 3d positions in depth camera
Point3D p0 = convertSkeletonPointToDepthPoint(m_skeletonCalibPoints[0]);
Point3D p1 = convertSkeletonPointToDepthPoint(m_skeletonCalibPoints[1]);
Point3D p2 = convertSkeletonPointToDepthPoint(m_skeletonCalibPoints[2]);
Point3D p3 = convertSkeletonPointToDepthPoint(m_skeletonCalibPoints[3]);
//3d positions depth camera --> positions on a 2D plane
Vector3D v1 = p1 - p0;
v1.Normalize();
Vector3D v2 = p2 - p0;
v2.Normalize();
Vector3D planeNormalVec = Vector3D.CrossProduct(v1, v2);
planeNormalVec.Normalize();
Vector3D resultingPlaneNormal = new Vector3D(0, 0, 1);
m_groundPlaneTransform = Util.make_align_axis_matrix(resultingPlaneNormal, planeNormalVec);
Point3D p0OnPlane = m_groundPlaneTransform.Transform(p0);
Point3D p1OnPlane = m_groundPlaneTransform.Transform(p1);
Point3D p2OnPlane = m_groundPlaneTransform.Transform(p2);
Point3D p3OnPlane = m_groundPlaneTransform.Transform(p3);
//2d plane positions --> exact 2d square on screen (using perspective transform)
System.Drawing.PointF[] src = new System.Drawing.PointF[4];
src[0] = new System.Drawing.PointF((float)p0OnPlane.X, (float)p0OnPlane.Y);
src[1] = new System.Drawing.PointF((float)p1OnPlane.X, (float)p1OnPlane.Y);
src[2] = new System.Drawing.PointF((float)p2OnPlane.X, (float)p2OnPlane.Y);
src[3] = new System.Drawing.PointF((float)p3OnPlane.X, (float)p3OnPlane.Y);
System.Drawing.PointF[] dest = new System.Drawing.PointF[4];
dest[0] = new System.Drawing.PointF((float)m_calibPoints[0].X, (float)m_calibPoints[0].Y);
dest[1] = new System.Drawing.PointF((float)m_calibPoints[1].X, (float)m_calibPoints[1].Y);
dest[2] = new System.Drawing.PointF((float)m_calibPoints[2].X, (float)m_calibPoints[2].Y);
dest[3] = new System.Drawing.PointF((float)m_calibPoints[3].X, (float)m_calibPoints[3].Y);
Emgu.CV.Mat transform = Emgu.CV.CvInvoke.GetPerspectiveTransform(src, dest);
m_transform = new Emgu.CV.Matrix <double>(transform.Rows, transform.Cols, transform.NumberOfChannels);
transform.CopyTo(m_transform);
//test to see if resulting perspective transform is correct
//tResultx should be same as points in m_calibPoints
Point tResult0 = kinectToProjectionPoint(m_skeletonCalibPoints[0]);
Point tResult1 = kinectToProjectionPoint(m_skeletonCalibPoints[1]);
Point tResult2 = kinectToProjectionPoint(m_skeletonCalibPoints[2]);
Point tResult3 = kinectToProjectionPoint(m_skeletonCalibPoints[3]);
txtCalib.Text = tResult0.ToString(CultureInfo.InvariantCulture) + ";\n" +
tResult1.ToString(CultureInfo.InvariantCulture) + ";\n" +
tResult2.ToString(CultureInfo.InvariantCulture) + ";\n" +
tResult3.ToString(CultureInfo.InvariantCulture);
}
}
private void Calibrate()
{
if (m_skeletonCalibPoints.Count == m_calibPoints.Count
// We need at least for points to map a rectangular region.
&& m_skeletonCalibPoints.Count == 4)
{
//seketon 3D positions --> 3d positions in depth camera
Point3D p0 = ConvertSkeletonPointToDepthPoint(m_kinectSensor, m_skeletonCalibPoints[0]);
Point3D p1 = ConvertSkeletonPointToDepthPoint(m_kinectSensor, m_skeletonCalibPoints[1]);
Point3D p2 = ConvertSkeletonPointToDepthPoint(m_kinectSensor, m_skeletonCalibPoints[2]);
Point3D p3 = ConvertSkeletonPointToDepthPoint(m_kinectSensor, m_skeletonCalibPoints[3]);
//3d positions depth camera --> positions on a 2D plane
Vector3D v1 = p1 - p0;
v1.Normalize();
Vector3D v2 = p2 - p0;
v2.Normalize();
Vector3D planeNormalVec = Vector3D.CrossProduct(v1, v2);
planeNormalVec.Normalize();
Vector3D resultingPlaneNormal = new Vector3D(0, 0, 1);
m_groundPlaneTransform = Util.Make_align_axis_matrix(resultingPlaneNormal, planeNormalVec);
Point3D p0OnPlane = m_groundPlaneTransform.Transform(p0);
Point3D p1OnPlane = m_groundPlaneTransform.Transform(p1);
Point3D p2OnPlane = m_groundPlaneTransform.Transform(p2);
Point3D p3OnPlane = m_groundPlaneTransform.Transform(p3);
//2d plane positions --> exact 2d square on screen (using perspective transform)
System.Drawing.PointF[] src = new System.Drawing.PointF[4];
src[0] = new System.Drawing.PointF((float)p0OnPlane.X, (float)p0OnPlane.Y);
src[1] = new System.Drawing.PointF((float)p1OnPlane.X, (float)p1OnPlane.Y);
src[2] = new System.Drawing.PointF((float)p2OnPlane.X, (float)p2OnPlane.Y);
src[3] = new System.Drawing.PointF((float)p3OnPlane.X, (float)p3OnPlane.Y);
System.Drawing.PointF[] dest = new System.Drawing.PointF[4];
dest[0] = new System.Drawing.PointF((float)m_calibPoints[0].X, (float)m_calibPoints[0].Y);
dest[1] = new System.Drawing.PointF((float)m_calibPoints[1].X, (float)m_calibPoints[1].Y);
dest[2] = new System.Drawing.PointF((float)m_calibPoints[2].X, (float)m_calibPoints[2].Y);
dest[3] = new System.Drawing.PointF((float)m_calibPoints[3].X, (float)m_calibPoints[3].Y);
Emgu.CV.Mat transform = Emgu.CV.CvInvoke.GetPerspectiveTransform(src, dest);
m_transform = new Emgu.CV.Matrix <double>(transform.Rows, transform.Cols, transform.NumberOfChannels);
transform.CopyTo(m_transform);
m_calibrationStatus = CalibrationStep.Calibrated;
//test to see if resulting perspective transform is correct
//tResultx should be same as points in m_calibPoints
//Point tResult0 = KinectToProjectionPoint(m_skeletonCalibPoints[0]);
//Point tResult1 = KinectToProjectionPoint(m_skeletonCalibPoints[1]);
//Point tResult2 = KinectToProjectionPoint(m_skeletonCalibPoints[2]);
//Point tResult3 = KinectToProjectionPoint(m_skeletonCalibPoints[3]);
//Debug.Assert(tResult0.Equals(m_calibPoints[0]));
//Debug.Assert(tResult1.Equals(m_calibPoints[1]));
//Debug.Assert(tResult2.Equals(m_calibPoints[2]));
//Debug.Assert(tResult3.Equals(m_calibPoints[3]));
}
}
