public Solve() { // x + y + z = 6 // 2x - 3y + 4z = 8 // 4x + 4y - 4z = 0 double[] A = new double[]{ 1, 1, 1, 2, -3, 4, 4, 4, -4 }; double[] B = new double[]{ 6, 8, 0 }; CvMat matA = new CvMat(3, 3, MatrixType.F64C1, A); CvMat matB = new CvMat(3, 1, MatrixType.F64C1, B); // X = inv(A) * B CvMat matAInv = matA.Clone(); matA.Inv(matAInv); CvMat matX = matAInv * matB; Console.WriteLine("X = {0}", matX[0].Val0); Console.WriteLine("Y = {0}", matX[1].Val0); Console.WriteLine("Z = {0}", matX[2].Val0); Console.Read(); }
public Solve() { // x + y + z = 6 // 2x - 3y + 4z = 8 // 4x + 4y - 4z = 0 double[] A = new double[] { 1, 1, 1, 2, -3, 4, 4, 4, -4 }; double[] B = new double[] { 6, 8, 0 }; CvMat matA = new CvMat(3, 3, MatrixType.F64C1, A); CvMat matB = new CvMat(3, 1, MatrixType.F64C1, B); // X = inv(A) * B CvMat matAInv = matA.Clone(); matA.Inv(matAInv); CvMat matX = matAInv * matB; Console.WriteLine("X = {0}", matX[0].Val0); Console.WriteLine("Y = {0}", matX[1].Val0); Console.WriteLine("Z = {0}", matX[2].Val0); Console.Read(); }
private double[] Cross_intersection_3D(double[] Intersection, double[,] first_projcet, double[,] second_projcet, CvPoint Correspond_point, int contour_point_index, double red, double green, double blue, int left_image_number, int right_image_number, int contour_index_model_operation_number)//求空间点坐标 { //IplImage first_image = new IplImage(image2, LoadMode.Color); //根据《空间点三维重建新方法及其不确定性研究》论文完成 double[,] A = new double[4, 3] { { Intersection[1] * first_projcet[2, 0] - first_projcet[0, 0], Intersection[1] * first_projcet[2, 1] - first_projcet[0, 1], Intersection[1] * first_projcet[2, 2] - first_projcet[0, 2] }, { Intersection[2] * first_projcet[2, 0] - first_projcet[1, 0], Intersection[2] * first_projcet[2, 1] - first_projcet[1, 1], Intersection[2] * first_projcet[2, 2] - first_projcet[1, 2] }, { Correspond_point.X *second_projcet[2, 0] - second_projcet[0, 0], Correspond_point.X *second_projcet[2, 1] - second_projcet[0, 1], Correspond_point.X *second_projcet[2, 2] - second_projcet[0, 2] }, { Correspond_point.Y *second_projcet[2, 0] - second_projcet[1, 0], Correspond_point.Y *second_projcet[2, 1] - second_projcet[1, 1], Correspond_point.Y *second_projcet[2, 2] - second_projcet[1, 2] } }; double[,] y = new double[6, 1] { { first_projcet[0, 3] - Intersection[1] * first_projcet[2, 3] }, { first_projcet[1, 3] - Intersection[2] * first_projcet[2, 3] }, { second_projcet[0, 3] - (Correspond_point.X * second_projcet[2, 3]) }, { second_projcet[1, 3] - (Correspond_point.Y * second_projcet[2, 3]) }, { 0 }, { 0 } }; double[,] s1 = new double[1, 3] { { A[0, 1] * A[1, 2] - A[0, 2] * A[1, 1], A[1, 0] * A[0, 2] - A[0, 0] * A[1, 2], A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1] } }; double[,] s2 = new double[1, 3] { { A[2, 1] * A[3, 2] - A[2, 2] * A[3, 1], A[3, 0] * A[2, 2] - A[2, 0] * A[3, 2], A[2, 0] * A[3, 1] - A[3, 0] * A[2, 1] } }; double[,] D = new double[6, 6] { { A[0, 0], A[0, 1], A[0, 2], 0, 0, 0 }, { A[1, 0], A[1, 1], A[1, 2], 0, 0, 0 }, { 0, 0, 0, A[2, 0], A[2, 1], A[2, 2] }, { 0, 0, 0, A[3, 0], A[3, 1], A[3, 2] }, { s1[0, 0], s1[0, 1], s1[0, 2], -s1[0, 0], -s1[0, 1], -s1[0, 2] }, { s2[0, 0], s2[0, 1], s2[0, 2], -s2[0, 0], -s2[0, 1], -s2[0, 2] } }; CvMat D_mat = new CvMat(6, 6, MatrixType.F64C1, D); CvMat D1_mat = new CvMat(6, 1, MatrixType.F64C1, y); CvMat matAInv1 = new CvMat(6, 6, MatrixType.F64C1, D); matAInv1 = D_mat.Clone(); D_mat.Inv(matAInv1); CvMat result = new CvMat(6, 1, MatrixType.F64C1); result = matAInv1 * D1_mat; //CvMat result= matAInv1 * D1_mat; //double Xb = result[0].Val0; double Yb = result[1].Val0; double Zb = result[2].Val0; double Xc = result[3].Val0; double Yc = result[4].Val0; double Zc = result[5].Val0; //如果映射像素颜色则运行时间很慢 //CvScalar first_image_pixel; //int first_image_pixel_x = (int)Intersection[1]; //int first_image_pixel_y = (int)Intersection[2]; //first_image_pixel = Cv.Get2D(first_image, first_image_pixel_y, first_image_pixel_x);// //double[,] point_3D_location = new double[1, 10] { { contour_point_index, (Xb + Xc) / 2, (Yb + Yc) / 2, (Zb + Zc) / 2, 1, red, green, blue,left_image_number,right_image_number } }; double[] point_3D_location; point_3D_location = new double[11] { contour_point_index, Xc, Yc, Zc, 1, red, green, blue, left_image_number, right_image_number, contour_index_model_operation_number }; //可视线上 // point_3D_location = new double[1, 10] { { contour_point_index, Xb, Yb, Zb, 1, red, green, blue, left_image_number, right_image_number } };//参考图上 //point_3D_location = new double[1, 11] { { contour_point_index, (Xb + Xc) / 2, (Yb + Yc) / 2, (Zb + Zc) / 2, 1, red, green, blue, left_image_number, right_image_number, contour_index_model_operation_number } }; Cv.ReleaseMat(D_mat); Cv.ReleaseMat(D1_mat); Cv.ReleaseMat(matAInv1); Cv.ReleaseMat(result); //GC.Collect(); return(point_3D_location); }
private CvMat Computecorrespondepilines(double[,] first_projcet, double[,] second_projcet, double[,] right_contour_point)//求两幅图的极线,此算法在窄基线情况下成立,返回的是位于基线上的两个点(因为求交的时候是用点作为参数,而不是A,B,C) { //double[,] Epiline_point = new double[4, right_contour_point.GetLength(1)]; CvMat Epiline_point = new CvMat(4, right_contour_point.GetLength(1), MatrixType.F64C1); CvMat correspondent_lines = new CvMat(3, right_contour_point.GetLength(1), MatrixType.F64C1); CvMat FundamentalMat = new CvMat(3, 3, MatrixType.F64C1); //《根据投影矩阵求基础矩阵》网页来求基础矩阵 double[,] M11 = new double[3, 3] { { first_projcet[0, 0], first_projcet[0, 1], first_projcet[0, 2] }, { first_projcet[1, 0], first_projcet[1, 1], first_projcet[1, 2] }, { first_projcet[2, 0], first_projcet[2, 1], first_projcet[2, 2] } }; double[,] M21 = new double[3, 3] { { second_projcet[0, 0], second_projcet[0, 1], second_projcet[0, 2] }, { second_projcet[1, 0], second_projcet[1, 1], second_projcet[1, 2] }, { second_projcet[2, 0], second_projcet[2, 1], second_projcet[2, 2] } }; double[,] m1 = new double[3, 1] { { first_projcet[0, 3] }, { first_projcet[1, 3] }, { first_projcet[2, 3] } }; double[,] m2 = new double[3, 1] { { second_projcet[0, 3] }, { second_projcet[1, 3] }, { second_projcet[2, 3] } }; CvMat M11_mat = new CvMat(3, 3, MatrixType.F64C1, M11); CvMat M21_mat = new CvMat(3, 3, MatrixType.F64C1, M21); CvMat m1_mat = new CvMat(3, 1, MatrixType.F64C1, m1); CvMat m2_mat = new CvMat(3, 1, MatrixType.F64C1, m2); CvMat M11_matInv = M11_mat.Clone(); M11_mat.Inv(M11_matInv); CvMat temp3 = M21_mat * M11_matInv * m1_mat; double[,] temp3_arry = new double[3, 1] { { temp3[0, 0] }, { temp3[1, 0] }, { temp3[2, 0] } }; double[,] m_arry = new double[3, 1]; m_arry = MatrixSubtration(m2, temp3_arry); CvMat m_mat = new CvMat(3, 1, MatrixType.F64C1, m_arry); double[,] mx_mat_arry = new double[3, 3] { { 0, -m_mat[2, 0], m_mat[1, 0] }, { m_mat[2, 0], 0, -m_mat[0, 0] }, { -m_mat[1, 0], m_mat[0, 0], 0 } }; CvMat mx_mat = new CvMat(3, 3, MatrixType.F64C1, mx_mat_arry); //MessageBox.Show(m_mat.ToString()); //MessageBox.Show(mx_mat.ToString()); FundamentalMat = mx_mat * M21_mat * M11_matInv; CvMat matA = new CvMat(2, right_contour_point.GetLength(1), MatrixType.F64C1, right_contour_point); //将数组转换为矩阵,列表示点的个数 Cv.ComputeCorrespondEpilines(matA, 2, FundamentalMat, out correspondent_lines); //correspondent_lines的列表示点的个数,经过证明图像指数是2,将其极线映射到参考图 double A = 0, B = 0, C = 0; //方程系数 //double A1 = correspondent_lines[0, 0]; //double B1 = correspondent_lines[1, 0]; //double C1 = correspondent_lines[2, 0]; //double A2 = correspondent_lines[0, 1]; //double B2 = correspondent_lines[1, 1]; //double C2 = correspondent_lines[2, 1]; //double[] epipole_temp = new double[2]; //epipole_temp[0] = (-1) * (B2 * C1 - B1 * C2) / (A1 * B2 - A2 * B1); //epipole_temp[1] = (-1) * (A2 * C1 - A1 * C2) / (A2 * B1 - A1* B2); //epipole = epipole_temp; for (int i = 0; i < right_contour_point.GetLength(1); i++)//一个轮廓点对应一条极线(有些轮廓点无相应图像的极线),一条极线要获得其上两个点,因为求交要用 { A = correspondent_lines[0, i]; B = correspondent_lines[1, i]; C = correspondent_lines[2, i]; Epiline_point[0, i] = 0; Epiline_point[1, i] = ((-C) / B); Epiline_point[2, i] = ((-C) / A); Epiline_point[3, i] = 0; //if (i != right_contour_point.GetLength(1) - 1) //{ // A1 = correspondent_lines[0, i]; // B1 = correspondent_lines[1, i]; // C1 = correspondent_lines[2, i]; // A2 = correspondent_lines[0, i + 1]; // B2 = correspondent_lines[1, i + 1]; // C2 = correspondent_lines[2, i + 1]; // epipole_temp = new double[2]; // epipole_temp[0] = (-1) * (B2 * C1 - B1 * C2) / (A1 * B2 - A2 * B1); // epipole_temp[1] = (-1) * (A2 * C1 - A1 * C2) / (A2 * B1 - A1 * B2); //} }//轮询轮廓点的循环在此结束 //MessageBox.Show(correspondent_lines.ToString()); Cv.ReleaseMat(correspondent_lines); Cv.ReleaseMat(FundamentalMat); Cv.ReleaseMat(M11_mat); Cv.ReleaseMat(M21_mat); Cv.ReleaseMat(m1_mat); Cv.ReleaseMat(m2_mat); Cv.ReleaseMat(M11_matInv); Cv.ReleaseMat(temp3); Cv.ReleaseMat(m_mat); Cv.ReleaseMat(mx_mat); Cv.ReleaseMat(matA); return(Epiline_point); }