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])); } }