// Return a region of interest (_rect_roi) from within the image _image // This doesn't need to be its own function, but I had so much trouble // finding a method that didn't crash the program that I separated it. CvMat GetROI(CvMat _image, CvRect rect_roi) { // Get the region of interest CvMat img_roi; // Get the region of interest // Grab the region of interest using the mouse-drawn box _image.GetSubRect(out img_roi, rect_roi); return(img_roi); }
public static void Test() { CoordRotTransConversion crtc = new CoordRotTransConversion(); Random rand = new Random(); CvMat cov = new CvMat(4, 4, MatrixType.F64C1); cov.Zero(); cov[0, 3] = rand.NextDouble() * 200 - 500; cov[1, 3] = rand.NextDouble() * 200 - 500; cov[2, 3] = rand.NextDouble() * 200 - 500; cov[3, 3] = 1.0; CvMat rotateConversion; cov.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1); rotVector[0, 0] = rand.NextDouble() * 10 - 5; rotVector[0, 1] = rand.NextDouble() * 10 - 5; rotVector[0, 2] = rand.NextDouble() * 10 - 5; Cv.Rodrigues2(rotVector, rotateConversion); for (int i = 0; i < 100000; i++) { CvPoint3D64f from = new CvPoint3D64f(rand.NextDouble() * rand.NextDouble() * rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500); CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1); CvEx.FillCvMat(fromMat, new double[] { from.X, from.Y, from.Z, 1.0 }); CvMat toMat = cov * fromMat; CvPoint3D64f to = new CvPoint3D64f(toMat[0, 0], toMat[0, 1], toMat[0, 2]); crtc.PutPoint(from, to, 1.0); } CvMat ret = crtc.Solve(); Func <CvMat, CvMat, string> show = (i, o) => { StringBuilder str = new StringBuilder(); str.AppendFormat("{0} = {1} / {2}\n", "11", i[0, 0].ToString("0.000"), o[0, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "12", i[0, 1].ToString("0.000"), o[0, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "13", i[0, 2].ToString("0.000"), o[0, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "14", i[0, 3].ToString("0.000"), o[0, 3].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "21", i[1, 0].ToString("0.000"), o[1, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "22", i[1, 1].ToString("0.000"), o[1, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "23", i[1, 2].ToString("0.000"), o[1, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "24", i[1, 3].ToString("0.000"), o[1, 3].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "31", i[2, 0].ToString("0.000"), o[2, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "32", i[2, 1].ToString("0.000"), o[2, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "33", i[2, 2].ToString("0.000"), o[2, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "34", i[2, 3].ToString("0.000"), o[2, 3].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "41", i[3, 0].ToString("0.000"), o[3, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "42", i[3, 1].ToString("0.000"), o[3, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "43", i[3, 2].ToString("0.000"), o[3, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "44", i[3, 3].ToString("0.000"), o[3, 3].ToString("0.000")); return(str.ToString()); }; MessageBox.Show(show(cov, ret)); }
public static void Test() { CoordRotTransConversion crtc = new CoordRotTransConversion(); Random rand = new Random(); CvMat cov = new CvMat(4, 4, MatrixType.F64C1); cov.Zero(); cov[0, 3] = rand.NextDouble() * 200 - 500; cov[1, 3] = rand.NextDouble() * 200 - 500; cov[2, 3] = rand.NextDouble() * 200 - 500; cov[3, 3] = 1.0; CvMat rotateConversion; cov.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1); rotVector[0, 0] = rand.NextDouble() * 10 - 5; rotVector[0, 1] = rand.NextDouble() * 10 - 5; rotVector[0, 2] = rand.NextDouble() * 10 - 5; Cv.Rodrigues2(rotVector, rotateConversion); for (int i = 0; i < 100000; i++) { CvPoint3D64f from = new CvPoint3D64f(rand.NextDouble() * rand.NextDouble() * rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500); CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1); CvEx.FillCvMat(fromMat, new double[] { from.X, from.Y, from.Z, 1.0 }); CvMat toMat = cov * fromMat; CvPoint3D64f to = new CvPoint3D64f(toMat[0, 0], toMat[0, 1], toMat[0, 2]); crtc.PutPoint(from, to, 1.0); } CvMat ret = crtc.Solve(); Func<CvMat, CvMat, string> show = (i, o) => { StringBuilder str = new StringBuilder(); str.AppendFormat("{0} = {1} / {2}\n", "11", i[0, 0].ToString("0.000"), o[0, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "12", i[0, 1].ToString("0.000"), o[0, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "13", i[0, 2].ToString("0.000"), o[0, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "14", i[0, 3].ToString("0.000"), o[0, 3].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "21", i[1, 0].ToString("0.000"), o[1, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "22", i[1, 1].ToString("0.000"), o[1, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "23", i[1, 2].ToString("0.000"), o[1, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "24", i[1, 3].ToString("0.000"), o[1, 3].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "31", i[2, 0].ToString("0.000"), o[2, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "32", i[2, 1].ToString("0.000"), o[2, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "33", i[2, 2].ToString("0.000"), o[2, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "34", i[2, 3].ToString("0.000"), o[2, 3].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "41", i[3, 0].ToString("0.000"), o[3, 0].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "42", i[3, 1].ToString("0.000"), o[3, 1].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "43", i[3, 2].ToString("0.000"), o[3, 2].ToString("0.000")); str.AppendFormat("{0} = {1} / {2}\n", "44", i[3, 3].ToString("0.000"), o[3, 3].ToString("0.000")); return str.ToString(); }; MessageBox.Show(show(cov, ret)); }
// Return a region of interest (_rect_roi) from within the image _image // This doesn't need to be its own function, but I had so much trouble // finding a method that didn't crash the program that I separated it. CvMat GetROI(CvMat _image, CvRect rect_roi) { // Get the region of interest CvMat img_roi; // Get the region of interest // Grab the region of interest using the mouse-drawn box _image.GetSubRect(out img_roi, rect_roi); return (img_roi); }
// => inputMat MUST be 24/32 bit private CvMat processFrame(CvMat inputMat) { // return "inputMat" after lots. LOTS. Of processing width = inputMat.Cols; height = inputMat.Rows; // taking out 4% of the input's edges: sounds wrong #if false // I have no idea what on earth is the purpose of this: //CvMat temp2 = inputMat( new CvRect( inputMat.Cols / 25, inputMat.Cols / 25, inputMat.Cols - 2 * (inputMat.Cols / 25), inputMat.Rows - 2 * (inputMat.Rows / 25) ) ); //resize( temp2, temp2, inputMat.size() ); //temp2.copyTo( inputMat ); int borderX = inputMat.Cols / 25; // 4% of original int borderY = inputMat.Rows / 25; CvRect roi = new CvRect(borderX, borderY, inputMat.Cols - 2 * borderX, inputMat.Rows - 2 * borderY); CvMat temp2 = inputMat.GetSubRect(out temp2, roi); // stupid to pass "out temp2"? inputMat = temp2; // =TODO : What? temp2.Copy( inputMat ); // is it really required to remove 4% of the input image's edges? #endif CvMat inputMat_grey; { // TODO : looks like a waste to make two conversions from inputMat to _grey, instead of 1 // since OpenCV doesn't support it, it could be made manually CvMat inputMat_grey8 = MatOps.ConvertChannels(inputMat); inputMat_grey = MatOps.ConvertElements(inputMat_grey8, MatrixType.F32C1, 1.0 / 255.0); } // NOTE : IBO seems to give good contrast with certain images, but with bbox7, it is just disastrous. //MatOps.NewWindowShow( inputMat_grey ); //inputMat_grey = Filters.IBO( inputMat_grey ); // inputMat_grey = 32f //MatOps.NewWindowShow( inputMat_grey ); inputMat_grey = MatOps.ConvertElements(inputMat_grey, MatrixType.U8C1, 255); // inputMat_grey = 8u // was: SLOW : Filters.ContrastEnhancement( inputMat_grey ); // NOTE : not needed AFTER IBO // NOTE : Contrast Enhancement2 may NOT be needed AT ALL, at this point at least, ANYWAY!!! Filters.ContrastEnhancement2(inputMat_grey); // NOTE : certainly NOT needed AFTER IBO MatOps.NewWindowShow(inputMat_grey); // mask passed originally in method below was all white, so I optimized it out. Passing the number of pixels was also dumb-o. double thresh = Filters.NeighborhoodValleyEmphasis(inputMat_grey); Cv.Threshold(inputMat_grey, inputMat_grey, thresh, 255, ThresholdType.BinaryInv); IplConvKernel element = new IplConvKernel(3, 3, 1, 1, ElementShape.Cross); Cv.Erode(inputMat_grey, inputMat_grey, element); Cv.Dilate(inputMat_grey, inputMat_grey, element); MatOps.NewWindowShow(inputMat_grey); // TODO : check if check is required if (inputMat_grey.ElemType != MatrixType.U8C1) { inputMat_grey = MatOps.ConvertElements(inputMat_grey, MatrixType.U8C1, 255.0); } // ======= // is this just a test? CvPoint[] newPtV = Filters.DistillContours(inputMat_grey, 5, Const.PointZero); CvMat imageDest; using (CvMemStorage storage = new CvMemStorage()) { CvSeq <CvPoint> updateContours = CvSeq <CvPoint> .FromArray(newPtV, SeqType.Contour, storage); imageDest = new CvMat(inputMat.Rows, inputMat.Cols, MatrixType.U8C1); Cv.DrawContours(imageDest, updateContours, Const.ScalarWhite, 0, 100, 16); } // ======= kawane(newPtV); // updates thresholdDist, minMaskY, final4P //*******************************************set a greater contour for estimation of the missing points*******************************// // ======= newPtV = Filters.DistillContours(inputMat_grey, 100, Const.PointZero); using (CvMemStorage storage = new CvMemStorage()) { CvSeq <CvPoint> updateContours = CvSeq <CvPoint> .FromArray(newPtV, SeqType.Contour, storage); imageDest = new CvMat(inputMat.Rows, inputMat.Cols, MatrixType.U8C1); Cv.DrawContours(imageDest, updateContours, Const.ScalarWhite, 0, 100, 1, LineType.AntiAlias); } // ======= CvMat mask1 = new CvMat(inputMat.Rows, inputMat.Cols, MatrixType.U8C1, 0); Cv.FillConvexPoly(mask1, newPtV, Const.ScalarWhite, 0, 0); temp = MatOps.ConvertChannels(inputMat); temp.Copy(imageDest, mask1); Cv.Canny(imageDest, imageDest, 150, 300, ApertureSize.Size3); IplConvKernel element2 = new IplConvKernel(3, 3, 1, 1, ElementShape.Rect); Cv.Dilate(imageDest, imageDest, element2); Cv.Erode(imageDest, imageDest, element2); CvLineSegmentPoint[] lines = Cv2.HoughLinesP(new Mat(imageDest), 1, Cv.PI / 180 /*NOTE : 1 degree angle*/, 50, 50, 50); // TODO : those 50s..? extendLines(lines, 350); // TODO : This idea sounds arbitary? And why 350? At least some percentage? // draw extended lines for (int i = 0; i < lines.Length; ++i) { CvLineSegmentPoint l = lines[i]; Cv.Line(imageDest, l.P1, l.P2, Const.ScalarWhite, 1, LineType.AntiAlias); } Cv.Dilate(imageDest, imageDest, element2); // TODO : FIX : Dilate again?! // another huge function here... fourPoints(lines); //////////// //********************************************************************* replace estimate points with mask corners ********// if (oldPt.Count != 0) { //** // BEWARE : great use of the English language following right below: // test for each and every one of the last slice delete each one of all the revisited of the above and estimate for only the best the off topic adapt //** List <int> positions = new List <int>(final4P.Count); for (int i = 0; i < final4P.Count; ++i) { positions.Add(-1); // "initialize" positions[i] double distmin = 10000; for (int j = 0; j < oldPt.Count; ++j) { double distAB = PointOps.Norm(oldPt[j] - final4P[i]); if (distAB < distmin) { distmin = distAB; positions[i] = j; } } } int flagFrCounter = 0; for (int i = 0; i < final4P.Count; ++i) { double distA = PointOps.Norm(oldPt[positions[i]] - final4P[i]); //********************* threshold pou na orizei tin megisti perioxi gia anazitisi,alliws na krataei to proigoumeno simeio*******// if (distA < thresholdDist) //if(distA<80) { oldPt[positions[i]] = final4P[i]; --flagFrCounter; } ++flagFrCounter; } if (reset) { numFrames = 0; oldPt.Clear(); final4P.Clear(); } } //pointsb[0]=thresholdDist; //****************************************************************************// for (int i = 0; i < oldPt.Count; ++i) { Cv.Circle(temp, oldPt[i], 2, Const.ScalarRed, 3); } MatOps.Convert8To24(temp).Copy(inputMat); //MatOps.ConvertChannels( temp, ColorConversion.GrayToBgr ).Copy( inputMat ); //temp.Copy( inputMat ); //******************************************************OVERLAY IMAGE***********************************************////// if (oldPt.Count == 0) { return(inputMat); // end of line } CvMat black2; if (overlay != null) { black2 = overlay.Clone(); //=imread("cubes.jpg"); Cv.Resize(black2, inputMat, Interpolation.NearestNeighbor); // TODO : check if interpolation type is appropriate } else { black2 = new CvMat(inputMat.Rows, inputMat.Cols, MatrixType.U8C3); } List <CvPoint> tempPoint = new List <CvPoint>(4); //vector<Point> tempPoint; int pp = 0; // BEWARE : the guy is copy/pasting needlessly? int mini = 1000000; for (int i = 0; i < oldPt.Count; ++i) { if (oldPt[i].Y < mini) { mini = oldPt[i].Y; pp = i; } } tempPoint.Add(oldPt[pp]); mini = 1000000; for (int i = 0; i < oldPt.Count; ++i) { if (oldPt[i].Y < mini && oldPt[i] != tempPoint[0]) { mini = oldPt[i].Y; pp = i; } } tempPoint.Add(oldPt[pp]); mini = 1000000; for (int i = 0; i < oldPt.Count; ++i) { int tempmini = Math.Abs(oldPt[i].X - tempPoint[1].X); if (tempmini < mini && oldPt[i] != tempPoint[0] && oldPt[i] != tempPoint[1]) { mini = tempmini; pp = i; } } tempPoint.Add(oldPt[pp]); for (int i = 0; i < oldPt.Count; ++i) { CvPoint pt = oldPt[i]; bool found = false; for (int j = 0; j < tempPoint.Count; ++j) { if (tempPoint[j] == pt) { found = true; break; } } if (!found) { tempPoint.Add(pt); } } // only keep up to 4 points List <CvPoint> co_ordinates = new List <CvPoint>(4); { int maxIndex = Math.Min(4, tempPoint.Count); for (int i = 0; i < maxIndex; ++i) { co_ordinates.Add(tempPoint[i]); } } // lost me... if (outputQuad[0] == outputQuad[2]) { { int maxIndex = Math.Min(4, tempPoint.Count); for (int i = 0; i < maxIndex; ++i) { outputQuad[i] = tempPoint[i]; } } } else { CvPoint2D32f rr; for (int i = 0; i < 4; ++i) { List <double> dist = new List <double>(tempPoint.Count); for (int j = 0; j < tempPoint.Count; ++j) { rr = tempPoint[j]; dist.Add(PointOps.Norm(outputQuad[i] - rr)); } double minimumDist = dist.Min(); int min_pos = Utils.FindIndex(dist, minimumDist); if (tempPoint.Count > 0) { outputQuad[i] = tempPoint[min_pos]; tempPoint.RemoveAt(min_pos); } } } // The 4 points where the mapping is to be done , from top-left in clockwise order inputQuad[0] = new CvPoint2D32f(0, 0); inputQuad[1] = new CvPoint2D32f(inputMat.Cols - 1, 0); inputQuad[2] = new CvPoint2D32f(inputMat.Cols - 1, inputMat.Rows - 1); inputQuad[3] = new CvPoint2D32f(0, inputMat.Rows - 1); //Input and Output Image; // Get the Perspective Transform Matrix i.e. lambda (2D warp transform) // Lambda Matrix CvMat lambda = Cv.GetPerspectiveTransform(inputQuad, outputQuad); // Apply this Perspective Transform to the src image // - get a "top-down" view of the supposedly box-y area Cv.WarpPerspective(black2, black2, lambda, Interpolation.Cubic, Const.ScalarBlack); // see nice explanation : http://www.pyimagesearch.com/2014/08/25/4-point-opencv-getperspective-transform-example/ CvMat maskOV = new CvMat(inputMat.Rows, inputMat.Cols, MatrixType.U8C1, Const.ScalarBlack); using (CvMemStorage storage = new CvMemStorage()) { CvSeq <CvPoint> updateContours = CvSeq <CvPoint> .FromArray(co_ordinates, SeqType.Contour, storage); imageDest = new CvMat(inputMat.Rows, inputMat.Cols, MatrixType.U8C1); Cv.DrawContours(maskOV, updateContours, Const.ScalarWhite, 0, 100, 16); //drawContours( maskOV, co_ordinates, 0, Scalar( 255 ), CV_FILLED, 8 ); } double alpha = 0.8; double beta = (1.0 - alpha); Cv.AddWeighted(black2, alpha, inputMat, beta, 0.0, black2); black2.Copy(inputMat, maskOV); return(inputMat); }
public CvMat Solve() { // 重心の計算 CvPoint3D64f fromCenter = new CvPoint3D64f(); CvPoint3D64f toCenter = new CvPoint3D64f(); double weightSum = 0; foreach (var tuple in _correspondings) { fromCenter += tuple.Item1 * tuple.Item3; toCenter += tuple.Item2 * tuple.Item3; weightSum += tuple.Item3; } if (weightSum != 0) { fromCenter *= 1.0 / weightSum; toCenter *= 1.0 / weightSum; } // q: quaternion; 4x1 // fn, tn: from[n], to[n]; 3x1 // Xn: (tn - fn, (tn+fn)×[1,0,0], (tn+fn)×[0,1,0], (tn+fn)×[0,0,1]); 3x4 // M: Σi(Xi^t Wi Xi); 4x4 // Wi: I; 3x3 // J = q^t Mq -> min // 最小二乗法 using (CvMat M = new CvMat(4, 4, MatrixType.F64C1)) { M.Zero(); foreach (var tuple in _correspondings) { // 重心からの距離 CvPoint3D64f fromVector = tuple.Item1 - fromCenter; CvPoint3D64f toVector = tuple.Item2 - toCenter; using (CvMat Xi = new CvMat(3, 4, MatrixType.F64C1)) { CvPoint3D64f diff = toVector - fromVector; CvPoint3D64f sum = toVector + fromVector; CvPoint3D64f second = CvEx.Cross(sum, new CvPoint3D64f(1, 0, 0)); CvPoint3D64f third = CvEx.Cross(sum, new CvPoint3D64f(0, 1, 0)); CvPoint3D64f fourth = CvEx.Cross(sum, new CvPoint3D64f(0, 0, 1)); CvEx.FillCvMat(Xi, new double[] { diff.X, second.X, third.X, fourth.X, diff.Y, second.Y, third.Y, fourth.Y, diff.Z, second.Z, third.Z, fourth.Z }); using (CvMat XiTranspose = Xi.Transpose()) using (CvMat addend = XiTranspose * Xi * tuple.Item3) { M.Add(addend, M); } } } using (CvMat MTemp = CvEx.CloneCvMat(M)) using (CvMat eVals = new CvMat(4, 1, MatrixType.F64C1)) using (CvMat eVects = new CvMat(4, 4, MatrixType.F64C1)) { //Cv.EigenVV(MTemp, eVects, eVals, 0.000001); Cv.SVD(MTemp, eVals, eVects, null, SVDFlag.U_T | SVDFlag.ModifyA); int minEIndex = 3; /* * if (false) * { * double minE = double.MaxValue; * for (int i = 0; i < 4; i++) * { * double eVal = Math.Abs(eVals[i, 0]); * if (eVal < minE) * { * minE = eVal; * minEIndex = i; * } * } * } */ CvMat ret = new CvMat(4, 4, MatrixType.F64C1); ret.Zero(); ret[3, 3] = 1.0; CvMat rotateConversion; /* * if (false) * { * // こっちの変換はほとんど恒等のときに誤差が大きい * CvMat q = eVects.GetRow(minEIndex); * * // クォータニオンから回転ベクトルを計算 * double theta = Math.Acos(q[0, 0]) * 2; * double sin = Math.Sin(theta / 2); * CvPoint3D64f rot = new CvPoint3D64f(q[0, 1] / sin * theta, q[0, 2] / sin * theta, q[0, 3] / sin * theta); * // 回転ベクトルから回転行列を計算 * ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); * using (CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1)) * { * rotVector[0, 0] = rot.X; * rotVector[0, 1] = rot.Y; * rotVector[0, 2] = rot.Z; * Cv.Rodrigues2(rotVector, rotateConversion); * } * } * else * {*/ CvMat rotationMat = CvEx.QuaternionToMat3D(eVects[minEIndex, 0], eVects[minEIndex, 1], eVects[minEIndex, 2], eVects[minEIndex, 3]); ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); rotationMat.Copy(rotateConversion); //} // 回転後の重心の並進成分の計算 using (CvMat fromCenterMat = new CvMat(3, 1, MatrixType.F64C1)) { CvEx.FillCvMat(fromCenterMat, new double[] { fromCenter.X, fromCenter.Y, fromCenter.Z }); using (CvMat rotFromCenterMat = rotateConversion * fromCenterMat) { CvPoint3D64f rotFromCenter = new CvPoint3D64f(rotFromCenterMat[0, 0], rotFromCenterMat[1, 0], rotFromCenterMat[2, 0]); CvPoint3D64f offset = toCenter - rotFromCenter; ret[0, 3] = offset.X; ret[1, 3] = offset.Y; ret[2, 3] = offset.Z; return(ret); } } } } }
public CvMat Solve() { // 重心の計算 CvPoint3D64f fromCenter = new CvPoint3D64f(); CvPoint3D64f toCenter = new CvPoint3D64f(); double weightSum = 0; foreach (var tuple in _correspondings) { fromCenter += tuple.Item1 * tuple.Item3; toCenter += tuple.Item2 * tuple.Item3; weightSum += tuple.Item3; } if (weightSum != 0) { fromCenter *= 1.0 / weightSum; toCenter *= 1.0 / weightSum; } // q: quaternion; 4x1 // fn, tn: from[n], to[n]; 3x1 // Xn: (tn - fn, (tn+fn)×[1,0,0], (tn+fn)×[0,1,0], (tn+fn)×[0,0,1]); 3x4 // M: Σi(Xi^t Wi Xi); 4x4 // Wi: I; 3x3 // J = q^t Mq -> min // 最小二乗法 using (CvMat M = new CvMat(4, 4, MatrixType.F64C1)) { M.Zero(); foreach (var tuple in _correspondings) { // 重心からの距離 CvPoint3D64f fromVector = tuple.Item1 - fromCenter; CvPoint3D64f toVector = tuple.Item2 - toCenter; using (CvMat Xi = new CvMat(3, 4, MatrixType.F64C1)) { CvPoint3D64f diff = toVector - fromVector; CvPoint3D64f sum = toVector + fromVector; CvPoint3D64f second = CvEx.Cross(sum, new CvPoint3D64f(1, 0, 0)); CvPoint3D64f third = CvEx.Cross(sum, new CvPoint3D64f(0, 1, 0)); CvPoint3D64f fourth = CvEx.Cross(sum, new CvPoint3D64f(0, 0, 1)); CvEx.FillCvMat(Xi, new double[] { diff.X, second.X, third.X, fourth.X, diff.Y, second.Y, third.Y, fourth.Y, diff.Z, second.Z, third.Z, fourth.Z }); using (CvMat XiTranspose = Xi.Transpose()) using (CvMat addend = XiTranspose * Xi * tuple.Item3) { M.Add(addend, M); } } } using (CvMat MTemp = CvEx.CloneCvMat(M)) using (CvMat eVals = new CvMat(4, 1, MatrixType.F64C1)) using (CvMat eVects = new CvMat(4, 4, MatrixType.F64C1)) { //Cv.EigenVV(MTemp, eVects, eVals, 0.000001); Cv.SVD(MTemp, eVals, eVects, null, SVDFlag.U_T | SVDFlag.ModifyA); int minEIndex = 3; /* if (false) { double minE = double.MaxValue; for (int i = 0; i < 4; i++) { double eVal = Math.Abs(eVals[i, 0]); if (eVal < minE) { minE = eVal; minEIndex = i; } } } */ CvMat ret = new CvMat(4, 4, MatrixType.F64C1); ret.Zero(); ret[3, 3] = 1.0; CvMat rotateConversion; /* if (false) { // こっちの変換はほとんど恒等のときに誤差が大きい CvMat q = eVects.GetRow(minEIndex); // クォータニオンから回転ベクトルを計算 double theta = Math.Acos(q[0, 0]) * 2; double sin = Math.Sin(theta / 2); CvPoint3D64f rot = new CvPoint3D64f(q[0, 1] / sin * theta, q[0, 2] / sin * theta, q[0, 3] / sin * theta); // 回転ベクトルから回転行列を計算 ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); using (CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1)) { rotVector[0, 0] = rot.X; rotVector[0, 1] = rot.Y; rotVector[0, 2] = rot.Z; Cv.Rodrigues2(rotVector, rotateConversion); } } else {*/ CvMat rotationMat = CvEx.QuaternionToMat3D(eVects[minEIndex, 0], eVects[minEIndex, 1], eVects[minEIndex, 2], eVects[minEIndex, 3]); ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); rotationMat.Copy(rotateConversion); //} // 回転後の重心の並進成分の計算 using (CvMat fromCenterMat = new CvMat(3, 1, MatrixType.F64C1)) { CvEx.FillCvMat(fromCenterMat, new double[] { fromCenter.X, fromCenter.Y, fromCenter.Z }); using (CvMat rotFromCenterMat = rotateConversion * fromCenterMat) { CvPoint3D64f rotFromCenter = new CvPoint3D64f(rotFromCenterMat[0, 0], rotFromCenterMat[1, 0], rotFromCenterMat[2, 0]); CvPoint3D64f offset = toCenter - rotFromCenter; ret[0, 3] = offset.X; ret[1, 3] = offset.Y; ret[2, 3] = offset.Z; return ret; } } } } }