bool SVD(const Matrix4& A,Matrix4& U,Vector4& W,Matrix4& V)
{
Matrix mA;
Copy(A,mA);
Real scale=mA.maxAbsElement();
mA /= scale;
SVDecomposition<Real> svd;
if(!svd.set(mA)) {
return false;
}
svd.sortSVs();
Copy(svd.U,U);
Copy(svd.V,V);
Copy(svd.W,W);
W *= scale;
return true;
}
bool FitGaussian(const vector<Vector3>& pts,Vector3& mean,Matrix3& R,Vector3& axes)
{
mean = GetMean(pts);
Matrix A(pts.size(),3);
for(size_t i=0;i<pts.size();i++)
(pts[i]-mean).get(A(i,0),A(i,1),A(i,2));
SVDecomposition<Real> svd;
if(!svd.set(A)) {
return false;
}
svd.sortSVs();
axes.set(svd.W(0),svd.W(1),svd.W(2));
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
R(i,j) = svd.V(i,j);
return true;
}
bool FitPlane(const vector<Vector3>& pts,Plane3D& p)
{
Vector3 mean = GetMean(pts);
Matrix A(pts.size(),3);
for(size_t i=0;i<pts.size();i++)
(pts[i]-mean).get(A(i,0),A(i,1),A(i,2));
SVDecomposition<Real> svd;
if(!svd.set(A)) {
return false;
}
svd.sortSVs();
//take the last singular value
Vector sv;
svd.V.getColRef(2,sv);
p.normal.set(sv(0),sv(1),sv(2));
p.normal.inplaceNormalize();
p.offset = dot(p.normal,mean);
return true;
}
bool FitLine(const vector<Vector3>& pts,Line3D& l)
{
Vector3 mean = GetMean(pts);
Matrix A(pts.size(),3);
for(size_t i=0;i<pts.size();i++)
(pts[i]-mean).get(A(i,0),A(i,1),A(i,2));
SVDecomposition<Real> svd;
if(!svd.set(A)) {
return false;
}
svd.sortSVs();
//take the first singular value
Vector sv;
svd.V.getColRef(0,sv);
l.direction.set(sv(0),sv(1),sv(2));
l.direction.inplaceNormalize();
l.source = mean;
return true;
}
Real FitFrames(const std::vector<Vector3>& a,const std::vector<Vector3>& b,
RigidTransform& Ta,RigidTransform& Tb,Vector3& cov)
{
//center at centroid
Vector3 ca(Zero),cb(Zero);
for(size_t i=0;i<a.size();i++) ca += a[i];
ca /= a.size();
for(size_t i=0;i<b.size();i++) cb += b[i];
cb /= b.size();
Matrix3 C;
C.setZero();
for(size_t k=0;k<a.size();k++) {
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
C(i,j) += (a[k][i]-ca[i])*(b[k][j]-cb[j]); //note the difference from RotationFit
}
Matrix mC(3,3),mCtC(3,3);
Copy(C,mC);
SVDecomposition<Real> svd;
if(!svd.set(mC)) {
cerr<<"FitFrames: Couldn't set svd of covariance matrix"<<endl;
Ta.R.setIdentity();
Tb.R.setIdentity();
return Inf;
}
Copy(svd.U,Ta.R);
Copy(svd.V,Tb.R);
Copy(svd.W,cov);
Tb.R.inplaceTranspose();
Ta.R.inplaceTranspose();
if (Ta.R.determinant() < 0 || Tb.R.determinant() < 0) {
//need to sort singular values and negate the column according to the smallest SV
svd.sortSVs();
//negate the last column of V and last column of U
if(Tb.R.determinant() < 0) {
Vector vi;
svd.V.getColRef(2, vi);
vi.inplaceNegative();
}
if(Ta.R.determinant() < 0) {
Vector ui;
svd.U.getColRef(2, ui);
ui.inplaceNegative();
}
Copy(svd.U, Ta.R);
Copy(svd.V, Tb.R);
Copy(svd.W, cov);
Tb.R.inplaceTranspose();
Ta.R.inplaceTranspose();
//TODO: these may now both have a mirroring, but they may compose to rotations?
}
//need these to act as though they subtract off the centroids FIRST then apply rotation
Ta.t = -(Ta.R*ca);
Tb.t = -(Tb.R*cb);
Real sum=0;
for(size_t k=0;k<a.size();k++)
sum += (Tb.R*(b[k]-cb)).distanceSquared(Ta.R*(a[k]-ca));
return sum;
}
Real RotationFit(const vector<Vector3>& a,const vector<Vector3>& b,Matrix3& R)
{
Assert(a.size() == b.size());
assert(a.size() >= 3);
Matrix3 C;
C.setZero();
for(size_t k=0;k<a.size();k++) {
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
C(i,j) += a[k][j]*b[k][i];
}
//let A=[a1 ... an]^t, B=[b1 ... bn]^t
//solve for min sum of squares of E=ARt-B
//let C=AtB
//solution is given by CCt = RtCtCR
//Solve C^tR = R^tC with SVD CC^t = R^tC^tCR
//CtRX = RtCX
//C = RCtR
//Ct = RtCRt
//=> CCt = RCtCRt
//solve SVD of C and Ct (giving eigenvectors of CCt and CtC
//C = UWVt => Ct=VWUt
//=> UWUt = RVWVtRt
//=> U=RV => R=UVt
Matrix mC(3,3),mCtC(3,3);
Copy(C,mC);
SVDecomposition<Real> svd;
if(!svd.set(mC)) {
cerr<<"RotationFit: Couldn't set svd of covariance matrix"<<endl;
R.setIdentity();
return Inf;
}
Matrix mR;
mR.mulTransposeB(svd.U,svd.V);
Copy(mR,R);
if(R.determinant() < 0) { //it's a mirror
svd.sortSVs();
if(!FuzzyZero(svd.W(2),(Real)1e-2)) {
cerr<<"RotationFit: Uhh... what do we do? SVD of rotation doesn't have a zero singular value"<<endl;
/*
cerr<<svd.W<<endl;
cerr<<"Press any key to continue"<<endl;
getchar();
*/
}
//negate the last column of V
Vector vi;
svd.V.getColRef(2,vi);
vi.inplaceNegative();
mR.mulTransposeB(svd.V,svd.U);
Copy(mR,R);
Assert(R.determinant() > 0);
}
Real sum=0;
for(size_t k=0;k<a.size();k++)
sum += b[k].distanceSquared(R*a[k]);
return sum;
}
