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