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