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