/// <summary> /// /// </summary> /// <param name="dataFilename"></param> /// <param name="filenameToSave"></param> /// <param name="filenameToLoad"></param> private void BuildBoostClassifier(string dataFilename, string filenameToSave, string filenameToLoad) { const int ClassCount = 26; CvMat data = null; CvMat responses = null; CvMat varType = null; CvMat tempSample = null; CvMat weakResponses = null; int nsamplesAall = 0, ntrainSamples = 0; int varCount; double trainHr = 0, testHr = 0; CvBoost boost = new CvBoost(); try { ReadNumClassData(dataFilename, 16, out data, out responses); } catch { Console.WriteLine("Could not read the database {0}", dataFilename); return; } Console.WriteLine("The database {0} is loaded.", dataFilename); nsamplesAall = data.Rows; ntrainSamples = (int)(nsamplesAall * 0.5); varCount = data.Cols; // Create or load Boosted Tree classifier if (filenameToLoad != null) { // load classifier from the specified file boost.Load(filenameToLoad); ntrainSamples = 0; if (boost.GetWeakPredictors() == null) { Console.WriteLine("Could not read the classifier {0}", filenameToLoad); return; } Console.WriteLine("The classifier {0} is loaded.", filenameToLoad); } else { // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // // As currently boosted tree classifier in MLL can only be trained // for 2-class problems, we transform the training database by // "unrolling" each training sample as many times as the number of // classes (26) that we have. // // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! using (CvMat newData = new CvMat(ntrainSamples * ClassCount, varCount + 1, MatrixType.F32C1)) using (CvMat newResponses = new CvMat(ntrainSamples * ClassCount, 1, MatrixType.S32C1)) { // 1. unroll the database type mask Console.WriteLine("Unrolling the database..."); for (int i = 0; i < ntrainSamples; i++) { unsafe { float *dataRow = (float *)(data.DataByte + data.Step * i); for (int j = 0; j < ClassCount; j++) { float *newDataRow = (float *)(newData.DataByte + newData.Step * (i * ClassCount + j)); for (int k = 0; k < varCount; k++) { newDataRow[k] = dataRow[k]; } newDataRow[varCount] = (float)j; newResponses.DataInt32[i * ClassCount + j] = (responses.DataSingle[i] == j + 'A') ? 1 : 0; } } } // 2. create type mask varType = new CvMat(varCount + 2, 1, MatrixType.U8C1); varType.Set(CvScalar.ScalarAll(CvStatModel.CV_VAR_ORDERED)); // the last indicator variable, as well // as the new (binary) response are categorical varType.SetReal1D(varCount, CvStatModel.CV_VAR_CATEGORICAL); varType.SetReal1D(varCount + 1, CvStatModel.CV_VAR_CATEGORICAL); // 3. train classifier Console.Write("Training the classifier (may take a few minutes)..."); boost.Train( newData, DTreeDataLayout.RowSample, newResponses, null, null, varType, null, new CvBoostParams(CvBoost.REAL, 100, 0.95, 5, false, null) ); } Console.WriteLine(); } tempSample = new CvMat(1, varCount + 1, MatrixType.F32C1); weakResponses = new CvMat(1, boost.GetWeakPredictors().Total, MatrixType.F32C1); // compute prediction error on train and test data for (int i = 0; i < nsamplesAall; i++) { int bestClass = 0; double maxSum = double.MinValue; double r; CvMat sample; Cv.GetRow(data, out sample, i); for (int k = 0; k < varCount; k++) { tempSample.DataArraySingle[k] = sample.DataArraySingle[k]; } for (int j = 0; j < ClassCount; j++) { tempSample.DataArraySingle[varCount] = (float)j; boost.Predict(tempSample, null, weakResponses); double sum = weakResponses.Sum().Val0; if (maxSum < sum) { maxSum = sum; bestClass = j + 'A'; } } r = (Math.Abs(bestClass - responses.DataArraySingle[i]) < float.Epsilon) ? 1 : 0; if (i < ntrainSamples) { trainHr += r; } else { testHr += r; } } testHr /= (double)(nsamplesAall - ntrainSamples); trainHr /= (double)ntrainSamples; Console.WriteLine("Recognition rate: train = {0:F1}%, test = {1:F1}%", trainHr * 100.0, testHr * 100.0); Console.WriteLine("Number of trees: {0}", boost.GetWeakPredictors().Total); // Save classifier to file if needed if (filenameToSave != null) { boost.Save(filenameToSave); } Console.Read(); tempSample.Dispose(); weakResponses.Dispose(); if (varType != null) { varType.Dispose(); } data.Dispose(); responses.Dispose(); boost.Dispose(); }
/// <summary> /// RTrees /// </summary> /// <param name="dataFilename"></param> /// <param name="filenameToSave"></param> /// <param name="filenameToLoad"></param> private void BuildRtreesClassifier(string dataFilename, string filenameToSave, string filenameToLoad) { CvMat data = null; CvMat responses = null; CvMat varType = null; CvMat sampleIdx = null; int nsamplesAll = 0, ntrainSamples = 0; double trainHr = 0, testHr = 0; CvRTrees forest = new CvRTrees(); try { ReadNumClassData(dataFilename, 16, out data, out responses); } catch { Console.WriteLine("Could not read the database {0}", dataFilename); return; } Console.WriteLine("The database {0} is loaded.", dataFilename); nsamplesAll = data.Rows; ntrainSamples = (int)(nsamplesAll * 0.8); // Create or load Random Trees classifier if (filenameToLoad != null) { // load classifier from the specified file forest.Load(filenameToLoad); ntrainSamples = 0; if (forest.GetTreeCount() == 0) { Console.WriteLine("Could not read the classifier {0}", filenameToLoad); return; } Console.WriteLine("The classifier {0} is loaded.", filenameToLoad); } else { // create classifier by using <data> and <responses> Console.Write("Training the classifier ..."); // 1. create type mask varType = new CvMat(data.Cols + 1, 1, MatrixType.U8C1); varType.Set(CvScalar.ScalarAll(CvStatModel.CV_VAR_ORDERED)); varType.SetReal1D(data.Cols, CvStatModel.CV_VAR_CATEGORICAL); // 2. create sample_idx sampleIdx = new CvMat(1, nsamplesAll, MatrixType.U8C1); { CvMat mat; Cv.GetCols(sampleIdx, out mat, 0, ntrainSamples); mat.Set(CvScalar.RealScalar(1)); Cv.GetCols(sampleIdx, out mat, ntrainSamples, nsamplesAll); mat.SetZero(); } // 3. train classifier forest.Train( data, DTreeDataLayout.RowSample, responses, null, sampleIdx, varType, null, new CvRTParams(10, 10, 0, false, 15, null, true, 4, new CvTermCriteria(100, 0.01f)) ); Console.WriteLine(); } // compute prediction error on train and test data for (int i = 0; i < nsamplesAll; i++) { double r; CvMat sample; Cv.GetRow(data, out sample, i); r = forest.Predict(sample); r = Math.Abs((double)r - responses.DataArraySingle[i]) <= float.Epsilon ? 1 : 0; if (i < ntrainSamples) { trainHr += r; } else { testHr += r; } } testHr /= (double)(nsamplesAll - ntrainSamples); trainHr /= (double)ntrainSamples; Console.WriteLine("Recognition rate: train = {0:F1}%, test = {1:F1}%", trainHr * 100.0, testHr * 100.0); Console.WriteLine("Number of trees: {0}", forest.GetTreeCount()); // Print variable importance Mat varImportance0 = forest.GetVarImportance(); CvMat varImportance = varImportance0.ToCvMat(); if (varImportance != null) { double rtImpSum = Cv.Sum(varImportance).Val0; Console.WriteLine("var#\timportance (in %):"); for (int i = 0; i < varImportance.Cols; i++) { Console.WriteLine("{0}\t{1:F1}", i, 100.0f * varImportance.DataArraySingle[i] / rtImpSum); } } // Print some proximitites Console.WriteLine("Proximities between some samples corresponding to the letter 'T':"); { CvMat sample1, sample2; int[,] pairs = new int[, ] { { 0, 103 }, { 0, 106 }, { 106, 103 }, { -1, -1 } }; for (int i = 0; pairs[i, 0] >= 0; i++) { Cv.GetRow(data, out sample1, pairs[i, 0]); Cv.GetRow(data, out sample2, pairs[i, 1]); Console.WriteLine("proximity({0},{1}) = {2:F1}%", pairs[i, 0], pairs[i, 1], forest.GetProximity(sample1, sample2) * 100.0); } } // Save Random Trees classifier to file if needed if (filenameToSave != null) { forest.Save(filenameToSave); } Console.Read(); if (sampleIdx != null) { sampleIdx.Dispose(); } if (varType != null) { varType.Dispose(); } data.Dispose(); responses.Dispose(); forest.Dispose(); }
/// <summary> /// RTrees /// </summary> /// <param name="dataFilename"></param> /// <param name="filenameToSave"></param> /// <param name="filenameToLoad"></param> private void BuildRtreesClassifier(string dataFilename, string filenameToSave, string filenameToLoad) { CvMat data = null; CvMat responses = null; CvMat varType = null; CvMat sampleIdx = null; int nsamplesAll = 0, ntrainSamples = 0; double trainHr = 0, testHr = 0; CvRTrees forest = new CvRTrees(); try { ReadNumClassData(dataFilename, 16, out data, out responses); } catch { Console.WriteLine("Could not read the database {0}", dataFilename); return; } Console.WriteLine("The database {0} is loaded.", dataFilename); nsamplesAll = data.Rows; ntrainSamples = (int)(nsamplesAll * 0.8); // Create or load Random Trees classifier if (filenameToLoad != null) { // load classifier from the specified file forest.Load(filenameToLoad); ntrainSamples = 0; if (forest.GetTreeCount() == 0) { Console.WriteLine("Could not read the classifier {0}", filenameToLoad); return; } Console.WriteLine("The classifier {0} is loaded.", filenameToLoad); } else { // create classifier by using <data> and <responses> Console.Write("Training the classifier ..."); // 1. create type mask varType = new CvMat(data.Cols + 1, 1, MatrixType.U8C1); varType.Set(CvScalar.ScalarAll(CvStatModel.CV_VAR_ORDERED)); varType.SetReal1D(data.Cols, CvStatModel.CV_VAR_CATEGORICAL); // 2. create sample_idx sampleIdx = new CvMat(1, nsamplesAll, MatrixType.U8C1); { CvMat mat; Cv.GetCols(sampleIdx, out mat, 0, ntrainSamples); mat.Set(CvScalar.RealScalar(1)); Cv.GetCols(sampleIdx, out mat, ntrainSamples, nsamplesAll); mat.SetZero(); } // 3. train classifier forest.Train( data, DTreeDataLayout.RowSample, responses, null, sampleIdx, varType, null, new CvRTParams(10, 10, 0, false, 15, null, true, 4, new CvTermCriteria(100, 0.01f)) ); Console.WriteLine(); } // compute prediction error on train and test data for (int i = 0; i < nsamplesAll; i++) { double r; CvMat sample; Cv.GetRow(data, out sample, i); r = forest.Predict(sample); r = Math.Abs((double)r - responses.DataArraySingle[i]) <= float.Epsilon ? 1 : 0; if (i < ntrainSamples) trainHr += r; else testHr += r; } testHr /= (double)(nsamplesAll - ntrainSamples); trainHr /= (double)ntrainSamples; Console.WriteLine("Recognition rate: train = {0:F1}%, test = {1:F1}%", trainHr * 100.0, testHr * 100.0); Console.WriteLine("Number of trees: {0}", forest.GetTreeCount()); // Print variable importance Mat varImportance0 = forest.GetVarImportance(); CvMat varImportance = varImportance0.ToCvMat(); if (varImportance != null) { double rtImpSum = Cv.Sum(varImportance).Val0; Console.WriteLine("var#\timportance (in %):"); for (int i = 0; i < varImportance.Cols; i++) { Console.WriteLine("{0}\t{1:F1}", i, 100.0f * varImportance.DataArraySingle[i] / rtImpSum); } } // Print some proximitites Console.WriteLine("Proximities between some samples corresponding to the letter 'T':"); { CvMat sample1, sample2; int[,] pairs = new int[,] { { 0, 103 }, { 0, 106 }, { 106, 103 }, { -1, -1 } }; for (int i = 0; pairs[i, 0] >= 0; i++) { Cv.GetRow(data, out sample1, pairs[i, 0]); Cv.GetRow(data, out sample2, pairs[i, 1]); Console.WriteLine("proximity({0},{1}) = {2:F1}%", pairs[i, 0], pairs[i, 1], forest.GetProximity(sample1, sample2) * 100.0); } } // Save Random Trees classifier to file if needed if (filenameToSave != null) { forest.Save(filenameToSave); } Console.Read(); if (sampleIdx != null) sampleIdx.Dispose(); if (varType != null) varType.Dispose(); data.Dispose(); responses.Dispose(); forest.Dispose(); }
/// <summary> /// /// </summary> /// <param name="dataFilename"></param> /// <param name="filenameToSave"></param> /// <param name="filenameToLoad"></param> private void BuildBoostClassifier(string dataFilename, string filenameToSave, string filenameToLoad) { const int ClassCount = 26; CvMat data = null; CvMat responses = null; CvMat varType = null; CvMat tempSample = null; CvMat weakResponses = null; int nsamplesAall = 0, ntrainSamples = 0; int varCount; double trainHr = 0, testHr = 0; CvBoost boost = new CvBoost(); try { ReadNumClassData(dataFilename, 16, out data, out responses); } catch { Console.WriteLine("Could not read the database {0}", dataFilename); return; } Console.WriteLine("The database {0} is loaded.", dataFilename); nsamplesAall = data.Rows; ntrainSamples = (int)(nsamplesAall * 0.5); varCount = data.Cols; // Create or load Boosted Tree classifier if (filenameToLoad != null) { // load classifier from the specified file boost.Load(filenameToLoad); ntrainSamples = 0; if (boost.GetWeakPredictors() == null) { Console.WriteLine("Could not read the classifier {0}", filenameToLoad); return; } Console.WriteLine("The classifier {0} is loaded.", filenameToLoad); } else { // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // // As currently boosted tree classifier in MLL can only be trained // for 2-class problems, we transform the training database by // "unrolling" each training sample as many times as the number of // classes (26) that we have. // // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! using (CvMat newData = new CvMat(ntrainSamples * ClassCount, varCount + 1, MatrixType.F32C1)) using (CvMat newResponses = new CvMat(ntrainSamples * ClassCount, 1, MatrixType.S32C1)) { // 1. unroll the database type mask Console.WriteLine("Unrolling the database..."); for (int i = 0; i < ntrainSamples; i++) { unsafe { float* dataRow = (float*)(data.DataByte + data.Step * i); for (int j = 0; j < ClassCount; j++) { float* newDataRow = (float*)(newData.DataByte + newData.Step * (i * ClassCount + j)); for (int k = 0; k < varCount; k++) { newDataRow[k] = dataRow[k]; } newDataRow[varCount] = (float)j; newResponses.DataInt32[i * ClassCount + j] = (responses.DataSingle[i] == j + 'A') ? 1 : 0; } } } // 2. create type mask varType = new CvMat(varCount + 2, 1, MatrixType.U8C1); varType.Set(CvScalar.ScalarAll(CvStatModel.CV_VAR_ORDERED)); // the last indicator variable, as well // as the new (binary) response are categorical varType.SetReal1D(varCount, CvStatModel.CV_VAR_CATEGORICAL); varType.SetReal1D(varCount + 1, CvStatModel.CV_VAR_CATEGORICAL); // 3. train classifier Console.Write("Training the classifier (may take a few minutes)..."); boost.Train( newData, DTreeDataLayout.RowSample, newResponses, null, null, varType, null, new CvBoostParams(CvBoost.REAL, 100, 0.95, 5, false, null) ); } Console.WriteLine(); } tempSample = new CvMat(1, varCount + 1, MatrixType.F32C1); weakResponses = new CvMat(1, boost.GetWeakPredictors().Total, MatrixType.F32C1); // compute prediction error on train and test data for (int i = 0; i < nsamplesAall; i++) { int bestClass = 0; double maxSum = double.MinValue; double r; CvMat sample; Cv.GetRow(data, out sample, i); for (int k = 0; k < varCount; k++) { tempSample.DataArraySingle[k] = sample.DataArraySingle[k]; } for (int j = 0; j < ClassCount; j++) { tempSample.DataArraySingle[varCount] = (float)j; boost.Predict(tempSample, null, weakResponses); double sum = weakResponses.Sum().Val0; if (maxSum < sum) { maxSum = sum; bestClass = j + 'A'; } } r = (Math.Abs(bestClass - responses.DataArraySingle[i]) < float.Epsilon) ? 1 : 0; if (i < ntrainSamples) trainHr += r; else testHr += r; } testHr /= (double)(nsamplesAall - ntrainSamples); trainHr /= (double)ntrainSamples; Console.WriteLine("Recognition rate: train = {0:F1}%, test = {1:F1}%", trainHr * 100.0, testHr * 100.0); Console.WriteLine("Number of trees: {0}", boost.GetWeakPredictors().Total); // Save classifier to file if needed if (filenameToSave != null) { boost.Save(filenameToSave); } Console.Read(); tempSample.Dispose(); weakResponses.Dispose(); if (varType != null) varType.Dispose(); data.Dispose(); responses.Dispose(); boost.Dispose(); }
/// <summary> /// 既知の内部パラメータを用いて,それぞれのビューにおける外部パラメータを推定する. /// 3次元のオブジェクトの点とそれに対応する2次元投影点が指定されなければならない.この関数も逆投影誤差の最小化を行う. /// </summary> /// <param name="objectPoints">オブジェクトの点の配列.3xNまたはNx3でNはビューにおける点の数.</param> /// <param name="imagePoints">対応する画像上の点の配列.2xNまたはNx2でNはビューにおける点の数.</param> /// <param name="cameraMatrix">カメラ内部行列 (A) [fx 0 cx; 0 fy cy; 0 0 1]. </param> /// <param name="distCoeffs">歪み係数のベクトル.4x1または1x4 [k1, k2, p1, p2].nullの場合,歪み係数はすべて0 であるとする.</param> /// <param name="rvec">出力される 3x1 の回転ベクトル</param> /// <param name="tvec">出力される 3x1 の並進ベクトル</param> /// <param name="useExtrinsicGuess"></param> #else /// <summary> /// Finds extrinsic camera parameters for particular view /// </summary> /// <param name="objectPoints">The array of object points, 3xN or Nx3, where N is the number of points in the view. </param> /// <param name="imagePoints">The array of corresponding image points, 2xN or Nx2, where N is the number of points in the view. </param> /// <param name="cameraMatrix">The camera matrix (A) [fx 0 cx; 0 fy cy; 0 0 1]. </param> /// <param name="distCoeffs">The vector of distortion coefficients, 4x1 or 1x4 [k1, k2, p1, p2]. If it is NULL, all distortion coefficients are considered 0's. </param> /// <param name="rvec">The output 3x1 or 1x3 rotation vector (compact representation of a rotation matrix, see cvRodrigues2). </param> /// <param name="tvec">The output 3x1 or 1x3 translation vector. </param> /// <param name="useExtrinsicGuess"></param> #endif public static void FindExtrinsicCameraParams2Cs(CvMat objectPoints, CvMat imagePoints, CvMat cameraMatrix, CvMat distCoeffs, CvMat rvec, CvMat tvec, bool useExtrinsicGuess) { if (objectPoints == null) throw new ArgumentNullException("objectPoints"); if (imagePoints == null) throw new ArgumentNullException("imagePoints"); if (cameraMatrix == null) throw new ArgumentNullException("cameraMatrix"); if (rvec == null) throw new ArgumentNullException("rvec"); if (tvec == null) throw new ArgumentNullException("tvec"); //IntPtr distCoeffsPtr = ToPtr(distCoeffs); unsafe { const int maxIter = 20; double[] ar = new double[9] { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; double[] MM = new double[9], U = new double[9], V = new double[9], W = new double[3]; double* param = stackalloc double[6]; CvMat matA = new CvMat(3, 3, MatrixType.F64C1); CvMat _Ar = new CvMat(3, 3, MatrixType.F64C1, ar); CvMat matR = new CvMat(3, 3, MatrixType.F64C1); CvMat _r = new CvMat(3, 1, MatrixType.F64C1, new IntPtr(param)); CvMat _t = new CvMat(3, 1, MatrixType.F64C1, new IntPtr(param + 3)); CvMat _Mc = new CvMat(1, 3, MatrixType.F64C1); CvMat _MM = new CvMat(3, 3, MatrixType.F64C1, MM); CvMat matU = new CvMat(3, 3, MatrixType.F64C1, U); CvMat matV = new CvMat(3, 3, MatrixType.F64C1, V); CvMat matW = new CvMat(3, 1, MatrixType.F64C1, W); CvMat _param = new CvMat(6, 1, MatrixType.F64C1, new IntPtr(param)); CvMat _dpdr, _dpdt; if (!IS_MAT(objectPoints.CvPtr) || !IS_MAT(imagePoints.CvPtr) || !IS_MAT(cameraMatrix.CvPtr) || !IS_MAT(rvec.CvPtr) || !IS_MAT(tvec.CvPtr)) { throw new ArgumentException(); } int count = Math.Max(objectPoints.Cols, objectPoints.Rows); CvMat matM = new CvMat(1, count, MatrixType.F64C3); CvMat _m = new CvMat(1, count, MatrixType.F64C2); ConvertPointsHomogeneous(objectPoints, matM); ConvertPointsHomogeneous(imagePoints, _m); Convert(cameraMatrix, matA); if (!((rvec.ElemType == MatrixType.F64C1 || rvec.ElemType == MatrixType.F32C1) && (rvec.Rows == 1 || rvec.Cols == 1) && rvec.Rows * rvec.Cols * rvec.ElemChannels == 3)) { throw new ArgumentException(); } if (!((tvec.ElemType == MatrixType.F64C1 || tvec.ElemType == MatrixType.F32C1) && (tvec.Rows == 1 || tvec.Cols == 1) && tvec.Rows * tvec.Cols * tvec.ElemChannels == 3)) { throw new ArgumentException(); } CvMat _mn = new CvMat(1, count, MatrixType.F64C2); CvMat _Mxy = new CvMat(1, count, MatrixType.F64C2); // normalize image points // (unapply the intrinsic matrix transformation and distortion) UndistortPoints_(_m, _mn, matA, distCoeffs, null, _Ar); if (useExtrinsicGuess) { using (CvMat _r_temp = new CvMat(rvec.Rows, rvec.Cols, MatrixType.F64C1)) using (CvMat _t_temp = new CvMat(tvec.Rows, tvec.Cols, MatrixType.F64C1)) { Convert(rvec, _r_temp); Convert(tvec, _t_temp); for (int i = 0; i < Math.Max(rvec.Rows, rvec.Cols); i++) { param[i] = _r_temp.GetReal1D(i); param[i + 3] = _t_temp.GetReal1D(i); } } } else { CvScalar Mc = Avg(matM); _Mc[0] = Mc.Val0; _Mc[1] = Mc.Val1; _Mc[2] = Mc.Val2; Reshape(matM, matM, 1, count); MulTransposed(matM, _MM, true, _Mc); SVD(_MM, matW, null, matV, SVDFlag.ModifyA | SVDFlag.V_T); // initialize extrinsic parameters if (W[2] / W[1] < 1e-3 || count < 4) { // a planar structure case (all M's lie in the same plane) double[] h = new double[9]; CvMat R_transform = matV; CvMat T_transform = new CvMat(3, 1, MatrixType.F64C1); CvMat matH = new CvMat(3, 3, MatrixType.F64C1, h); CvMat _h1, _h2, _h3; if (V[2] * V[2] + V[5] * V[5] < 1e-10) SetIdentity(R_transform); if (Det(R_transform) < 0) Scale(R_transform, R_transform, -1); //GEMM(R_transform, _Mc, -1, null, 0, T_transform, GemmOperation.B_T); for (int r = 0; r < 3; r++) { for (int c = 0; c < 1; c++) { double sum = 0; for (int k = 0; k < 3; k++) { sum += R_transform.GetReal2D(r, k) * _Mc.GetReal2D(c, k); } T_transform.SetReal2D(r, c, sum * -1); } } for (int i = 0; i < count; i++) { double* Rp = R_transform.DataDouble; double* Tp = T_transform.DataDouble; double* src = matM.DataDouble + i * 3; double* dst = _Mxy.DataDouble + i * 2; dst[0] = Rp[0] * src[0] + Rp[1] * src[1] + Rp[2] * src[2] + Tp[0]; dst[1] = Rp[3] * src[0] + Rp[4] * src[1] + Rp[5] * src[2] + Tp[1]; } FindHomography_(_Mxy, _mn, matH); GetCol(matH, out _h1, 0); GetCol(matH, out _h2, 0); GetCol(matH, out _h3, 0); _h2.DataDouble += 1; _h3.DataDouble += 2; double h1_norm = Math.Sqrt(h[0] * h[0] + h[3] * h[3] + h[6] * h[6]); double h2_norm = Math.Sqrt(h[1] * h[1] + h[4] * h[4] + h[7] * h[7]); Scale(_h1, _h1, 1.0 / h1_norm); Scale(_h2, _h2, 1.0 / h2_norm); Scale(_h3, _t, 2.0 / (h1_norm + h2_norm)); CrossProduct(_h1, _h2, _h3); Rodrigues2_(matH, _r); Rodrigues2_(_r, matH); MatMulAdd(matH, T_transform, _t, _t); MatMul(matH, R_transform, matR); Rodrigues2_(matR, _r); } else { // non-planar structure. Use DLT method double[] LL = new double[12 * 12], LW = new double[12], LV = new double[12 * 12]; CvMat _LL = new CvMat(12, 12, MatrixType.F64C1, LL); CvMat _LW = new CvMat(12, 1, MatrixType.F64C1, LW); CvMat _LV = new CvMat(12, 12, MatrixType.F64C1, LV); CvMat _RR, _tt; CvPoint3D64f* M = (CvPoint3D64f*)matM.DataDouble; CvPoint2D64f* mn = (CvPoint2D64f*)_mn.DataDouble; CvMat matL = new CvMat(2 * count, 12, MatrixType.F64C1); double* L = matL.DataDouble; for (int i = 0; i < count; i++, L += 24) { double x = -mn[i].X, y = -mn[i].Y; L[0] = L[16] = M[i].X; L[1] = L[17] = M[i].Y; L[2] = L[18] = M[i].Z; L[3] = L[19] = 1.0; L[4] = L[5] = L[6] = L[7] = 0.0; L[12] = L[13] = L[14] = L[15] = 0.0; L[8] = x * M[i].X; L[9] = x * M[i].Y; L[10] = x * M[i].Z; L[11] = x; L[20] = y * M[i].X; L[21] = y * M[i].Y; L[22] = y * M[i].Z; L[23] = y; } MulTransposed(matL, _LL, true); SVD(_LL, _LW, null, _LV, SVDFlag.ModifyA | SVDFlag.V_T); double[] LV12 = new double[12]; Array.Copy(LV, 11 * 12, LV12, 0, 12); CvMat _RRt = new CvMat(3, 4, MatrixType.F64C1, LV12); GetCols(_RRt, out _RR, 0, 3); GetCol(_RRt, out _tt, 3); if (Det(_RR) < 0) Scale(_RRt, _RRt, -1); double sc = Norm(_RR); SVD(_RR, matW, matU, matV, SVDFlag.ModifyA | SVDFlag.U_T | SVDFlag.V_T); GEMM(matU, matV, 1, null, 0, matR, GemmOperation.A_T); Scale(_tt, _t, Norm(matR) / sc); Rodrigues2_(matR, _r); } } Cv.Reshape(matM, matM, 3, 1); Cv.Reshape(_mn, _mn, 2, 1); // refine extrinsic parameters using iterative algorithm CvLevMarq solver = new CvLevMarq(6, count * 2, new CvTermCriteria(maxIter, float.Epsilon), true); Copy(_param, solver.Param); /* Console.WriteLine("matM-----"); for (int i = 0; i < matM.Rows * matM.Cols; i++) { Console.WriteLine("{0}\t", matM[i].Val0); } Console.WriteLine("_mn-----"); for (int i = 0; i < _mn.Rows * _mn.Cols; i++) { Console.WriteLine(_mn[i].Val0); } Console.WriteLine("_param-----"); for (int i = 0; i < _param.Rows * _param.Cols; i++) { Console.WriteLine(_param[i].Val0); }*/ for (; ; ) { CvMat matJ, _err, __param; bool proceed = solver.Update(out __param, out matJ, out _err); Copy(__param, _param); if (!proceed || _err == null) break; Reshape(_err, _err, 2, 1); if (matJ != null) { GetCols(matJ, out _dpdr, 0, 3); GetCols(matJ, out _dpdt, 3, 6); ProjectPoints2(matM, _r, _t, matA, distCoeffs, _err, _dpdr, _dpdt, null, null, null); } else { ProjectPoints2(matM, _r, _t, matA, distCoeffs, _err, null, null, null, null, null); } Sub(_err, _m, _err); Reshape(_err, _err, 1, 2 * count); } Copy(solver.Param, _param); for (int i = 0; i < 3; i++) { rvec.SetReal1D(i, param[i]); tvec.SetReal1D(i, param[i + 3]); } } }