Box3<Real> GaussPointsFit3 (int iQuantity, const Vector3<Real>* akPoint)
{
Box3<Real> kBox(Vector3<Real>::ZERO,Vector3<Real>::UNIT_X,
Vector3<Real>::UNIT_Y,Vector3<Real>::UNIT_Z,(Real)1.0,(Real)1.0,
(Real)1.0);
// compute the mean of the points
kBox.Center = akPoint[0];
int i;
for (i = 1; i < iQuantity; i++)
{
kBox.Center += akPoint[i];
}
Real fInvQuantity = ((Real)1.0)/iQuantity;
kBox.Center *= fInvQuantity;
// compute the covariance matrix of the points
Real fSumXX = (Real)0.0, fSumXY = (Real)0.0, fSumXZ = (Real)0.0;
Real fSumYY = (Real)0.0, fSumYZ = (Real)0.0, fSumZZ = (Real)0.0;
for (i = 0; i < iQuantity; i++)
{
Vector3<Real> kDiff = akPoint[i] - kBox.Center;
fSumXX += kDiff.X()*kDiff.X();
fSumXY += kDiff.X()*kDiff.Y();
fSumXZ += kDiff.X()*kDiff.Z();
fSumYY += kDiff.Y()*kDiff.Y();
fSumYZ += kDiff.Y()*kDiff.Z();
fSumZZ += kDiff.Z()*kDiff.Z();
}
fSumXX *= fInvQuantity;
fSumXY *= fInvQuantity;
fSumXZ *= fInvQuantity;
fSumYY *= fInvQuantity;
fSumYZ *= fInvQuantity;
fSumZZ *= fInvQuantity;
// setup the eigensolver
Eigen<Real> kES(3);
kES(0,0) = fSumXX;
kES(0,1) = fSumXY;
kES(0,2) = fSumXZ;
kES(1,0) = fSumXY;
kES(1,1) = fSumYY;
kES(1,2) = fSumYZ;
kES(2,0) = fSumXZ;
kES(2,1) = fSumYZ;
kES(2,2) = fSumZZ;
kES.IncrSortEigenStuff3();
for (i = 0; i < 3; i++)
{
kBox.Extent[i] = kES.GetEigenvalue(i);
kES.GetEigenvector(i,kBox.Axis[i]);
}
return kBox;
}
Line2<Real> OrthogonalLineFit2 (int iQuantity, const Vector2<Real>* akPoint)
{
Line2<Real> kLine(Vector2<Real>::ZERO,Vector2<Real>::ZERO);
// compute the mean of the points
kLine.Origin = akPoint[0];
int i;
for (i = 1; i < iQuantity; i++)
{
kLine.Origin += akPoint[i];
}
Real fInvQuantity = ((Real)1.0)/iQuantity;
kLine.Origin *= fInvQuantity;
// compute the covariance matrix of the points
Real fSumXX = (Real)0.0, fSumXY = (Real)0.0, fSumYY = (Real)0.0;
for (i = 0; i < iQuantity; i++)
{
Vector2<Real> kDiff = akPoint[i] - kLine.Origin;
fSumXX += kDiff.X()*kDiff.X();
fSumXY += kDiff.X()*kDiff.Y();
fSumYY += kDiff.Y()*kDiff.Y();
}
fSumXX *= fInvQuantity;
fSumXY *= fInvQuantity;
fSumYY *= fInvQuantity;
// set up the eigensolver
Eigen<Real> kES(2);
kES(0,0) = fSumYY;
kES(0,1) = -fSumXY;
kES(1,0) = -fSumXY;
kES(1,1) = fSumXX;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff2();
// unit-length direction for best-fit line
kES.GetEigenvector(1,kLine.Direction);
return kLine;
}
Real QuadraticFit2 (int iQuantity, const Vector2<Real>* akPoint,
Real afCoeff[6])
{
Eigen<Real> kES(6);
int iRow, iCol;
for (iRow = 0; iRow < 6; iRow++)
{
for (iCol = 0; iCol < 6; iCol++)
{
kES(iRow,iCol) = (Real)0.0;
}
}
for (int i = 0; i < iQuantity; i++)
{
Real fX = akPoint[i].X();
Real fY = akPoint[i].Y();
Real fX2 = fX*fX;
Real fY2 = fY*fY;
Real fXY = fX*fY;
Real fX3 = fX*fX2;
Real fXY2 = fX*fY2;
Real fX2Y = fX*fXY;
Real fY3 = fY*fY2;
Real fX4 = fX*fX3;
Real fX2Y2 = fX*fXY2;
Real fX3Y = fX*fX2Y;
Real fY4 = fY*fY3;
Real fXY3 = fX*fY3;
kES(0,1) += fX;
kES(0,2) += fY;
kES(0,3) += fX2;
kES(0,4) += fY2;
kES(0,5) += fXY;
kES(1,3) += fX3;
kES(1,4) += fXY2;
kES(1,5) += fX2Y;
kES(2,4) += fY3;
kES(3,3) += fX4;
kES(3,4) += fX2Y2;
kES(3,5) += fX3Y;
kES(4,4) += fY4;
kES(4,5) += fXY3;
}
kES(0,0) = (Real)iQuantity;
kES(1,1) = kES(0,3);
kES(1,2) = kES(0,5);
kES(2,2) = kES(0,4);
kES(2,3) = kES(1,5);
kES(2,5) = kES(1,4);
kES(5,5) = kES(3,4);
for (iRow = 0; iRow < 6; iRow++)
{
for (iCol = 0; iCol < iRow; iCol++)
{
kES(iRow,iCol) = kES(iCol,iRow);
}
}
Real fInvQuantity = ((Real)1.0)/(Real)iQuantity;
for (iRow = 0; iRow < 6; iRow++)
{
for (iCol = 0; iCol < 6; iCol++)
{
kES(iRow,iCol) *= fInvQuantity;
}
}
kES.IncrSortEigenStuffN();
GVector<Real> kEVector = kES.GetEigenvector(0);
size_t uiSize = 6*sizeof(Real);
System::Memcpy(afCoeff,uiSize,(Real*)kEVector,uiSize);
// For exact fit, numeric round-off errors may make the minimum
// eigenvalue just slightly negative. Return absolute value since
// application may rely on the return value being nonnegative.
return Math<Real>::FAbs(kES.GetEigenvalue(0));
}
Real QuadraticCircleFit2 (int iQuantity, const Vector2<Real>* akPoint,
Vector2<Real>& rkCenter, Real& rfRadius)
{
Eigen<Real> kES(4);
int iRow, iCol;
for (iRow = 0; iRow < 4; iRow++)
{
for (iCol = 0; iCol < 4; iCol++)
{
kES(iRow,iCol) = (Real)0.0;
}
}
for (int i = 0; i < iQuantity; i++)
{
Real fX = akPoint[i].X();
Real fY = akPoint[i].Y();
Real fX2 = fX*fX;
Real fY2 = fY*fY;
Real fXY = fX*fY;
Real fR2 = fX2+fY2;
Real fXR2 = fX*fR2;
Real fYR2 = fY*fR2;
Real fR4 = fR2*fR2;
kES(0,1) += fX;
kES(0,2) += fY;
kES(0,3) += fR2;
kES(1,1) += fX2;
kES(1,2) += fXY;
kES(1,3) += fXR2;
kES(2,2) += fY2;
kES(2,3) += fYR2;
kES(3,3) += fR4;
}
kES(0,0) = (Real)iQuantity;
for (iRow = 0; iRow < 4; iRow++)
{
for (iCol = 0; iCol < iRow; iCol++)
{
kES(iRow,iCol) = kES(iCol,iRow);
}
}
Real fInvQuantity = ((Real)1.0)/(Real)iQuantity;
for (iRow = 0; iRow < 4; iRow++)
{
for (iCol = 0; iCol < 4; iCol++)
{
kES(iRow,iCol) *= fInvQuantity;
}
}
kES.IncrSortEigenStuffN();
GVector<Real> kEVector = kES.GetEigenvector(0);
Real fInv = ((Real)1.0)/kEVector[3]; // beware zero divide
Real afCoeff[3];
for (iRow = 0; iRow < 3; iRow++)
{
afCoeff[iRow] = fInv*kEVector[iRow];
}
rkCenter.X() = -((Real)0.5)*afCoeff[1];
rkCenter.Y() = -((Real)0.5)*afCoeff[2];
rfRadius = Math<Real>::Sqrt(Math<Real>::FAbs(rkCenter.X()*rkCenter.X() +
rkCenter.Y()*rkCenter.Y() - afCoeff[0]));
// For exact fit, numeric round-off errors may make the minimum
// eigenvalue just slightly negative. Return absolute value since
// application may rely on the return value being nonnegative.
return Math<Real>::FAbs(kES.GetEigenvalue(0));
}
Vector4T<PrimitiveType> PlaneFit(const Vector3T<PrimitiveType> pts[], size_t ptCount)
{
/*
Given a set of points in 3 space, find a plane that best matches them, using least
squares regression.
The algorithm and base implementation here is from Geometric Tools, LLC
Copyright (c) 1998-2010
Distributed under the Boost Software License, Version 1.0.
http://www.boost.org/LICENSE_1_0.txt
http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
see http://www.geometrictools.com/Documentation/LeastSquaresFitting.pdf for a
description. Note that this is the orthogonal regression version. There is also
a version for dealing with points of the form (x,y, f(x,y)) -- this is less
general (as it seeks to minimize delta z), but may provide suitable results in
most cases.
*/
// compute the mean of the points
auto kOrigin = Zero<Vector3T<PrimitiveType>>();
for (size_t i = 0; i < ptCount; i++)
kOrigin += pts[i];
PrimitiveType reciprocalCount = ((PrimitiveType)1.0)/ptCount;
kOrigin *= reciprocalCount;
// compute sums of products
PrimitiveType fSumXX = (PrimitiveType)0.0, fSumXY = (PrimitiveType)0.0, fSumXZ = (PrimitiveType)0.0;
PrimitiveType fSumYY = (PrimitiveType)0.0, fSumYZ = (PrimitiveType)0.0, fSumZZ = (PrimitiveType)0.0;
for (size_t i = 0; i < ptCount; i++)
{
auto kDiff = pts[i] - kOrigin;
fSumXX += kDiff[0]*kDiff[0];
fSumXY += kDiff[0]*kDiff[1];
fSumXZ += kDiff[0]*kDiff[2];
fSumYY += kDiff[1]*kDiff[1];
fSumYZ += kDiff[1]*kDiff[2];
fSumZZ += kDiff[2]*kDiff[2];
}
fSumXX *= reciprocalCount;
fSumXY *= reciprocalCount;
fSumXZ *= reciprocalCount;
fSumYY *= reciprocalCount;
fSumYZ *= reciprocalCount;
fSumZZ *= reciprocalCount;
// setup the eigensolver
Eigen<PrimitiveType> kES(3);
kES(0,0) = fSumXX;
kES(0,1) = fSumXY;
kES(0,2) = fSumXZ;
kES(1,0) = fSumXY;
kES(1,1) = fSumYY;
kES(1,2) = fSumYZ;
kES(2,0) = fSumXZ;
kES(2,1) = fSumYZ;
kES(2,2) = fSumZZ;
// compute eigenstuff, smallest eigenvalue is in last position
kES.DecrSortEigenStuff3();
// get plane normal
Vector3T<PrimitiveType> kNormal;
kES.GetEigenvector(2,kNormal);
// the minimum energy
return Expand( kNormal, -Dot( kNormal, kOrigin ) );
}
