bool MatrixEquation::LeastSquares_SVD(Vector& x) const
{
//svd automatically does the least-squares
Assert(IsValid());
SVDecomposition<Real> svd;
if(A.m <= A.n) {
if(!svd.set(A)) return false;
svd.backSub(b,x);
return true;
}
else {
if(!svd.set(A)) return false;
svd.backSub(b,x);
return true;
/*
cerr<<"Doing the transpose SVD"<<endl;
Matrix At; At.setRefTranspose(A);
if(!svd.set(At)) return false;
svd.getInverse(At);
cout<<"Result"<<endl<<MatrixPrinter(At)<<endl;
Matrix Ainv; Ainv.setRefTranspose(At);
Ainv.mul(b,x);
return true;
*/
}
}
bool MatrixEquation::Solve_SVD(Vector& x) const
{
Assert(A.isSquare());
Assert(A.n == b.n);
SVDecomposition<Real> svd;
if(!svd.set(A)) return false;
svd.backSub(b,x);
return true;
}
bool MatrixEquation::AllSolutions_SVD(Vector& x0,Matrix& N) const
{
if(A.n < A.m) {
cout<<"Warning: matrix is overconstrained"<<endl;
}
SVDecomposition<Real> svd;
if(!svd.set(A)) return false;
svd.backSub(b,x0);
svd.getNullspace(N);
return true;
}
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 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;
}
ConvergenceResult Root_Newton(VectorFieldFunction& f,const Vector& x0, Vector& x, int& iters, Real tolx, Real tolf)
{
//move in gradient direction to set f to 0
//f(x) ~= f(x0) + df/dx(x0)*(x-x0) + O(h^2)
//=> 0 = fx + J*(x-x0) => dx = -J^-1*fx
SVDecomposition<Real> svd;
Vector fx,p;
Matrix fJx;
if(&x != &x0) x = x0;
int maxIters=iters;
for (iters=0;iters<maxIters;iters++) {
f.PreEval(x);
f.Eval(x,fx);
f.Jacobian(x,fJx);
if(fx.maxAbsElement() <= tolf) return ConvergenceF;
if(!svd.set(fJx)) {
return ConvergenceError;
}
svd.backSub(fx,p);
x -= p;
if(p.maxAbsElement() <= tolx) return ConvergenceX;
}
return MaxItersReached;
}
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 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;
}
bool LP_InteriorPoint::Set(const LinearProgram& lp)
{
Matrix Aeq;
Vector beq;
int neq=0,nineq=0;
for(int i=0;i<lp.A.m;i++) {
if(lp.ConstraintType(i) == LinearProgram::Fixed) neq++;
else {
if(lp.HasLowerBound(lp.ConstraintType(i))) nineq++;
if(lp.HasUpperBound(lp.ConstraintType(i))) nineq++;
}
}
for(int i=0;i<lp.A.n;i++) {
if(lp.VariableType(i) == LinearProgram::Fixed) neq++;
else {
if(lp.HasLowerBound(lp.VariableType(i))) nineq++;
if(lp.HasUpperBound(lp.VariableType(i))) nineq++;
}
}
if(neq == 0) {
x0.clear();
N.clear();
((LinearProgram&)solver) = lp;
//solver.minimize is ignored by the solver
if(!solver.minimize) solver.c.inplaceNegative();
return true;
}
Aeq.resize(neq,lp.A.n);
beq.resize(neq);
neq=0;
for(int i=0;i<lp.A.m;i++) {
if(lp.ConstraintType(i)==LinearProgram::Fixed)
{
Vector Ai;
lp.A.getRowRef(i,Ai);
Aeq.copyRow(neq,Ai);
beq(neq) = lp.p(i);
neq++;
}
}
for (int i=0;i<lp.A.n;i++) {
if(lp.VariableType(i)==LinearProgram::Fixed)
{
Vector Aeqi;
Aeq.getRowRef(i,Aeqi);
Aeqi.setZero();
Aeqi(i) = One;
beq(neq) = lp.l(i);
neq++;
}
}
SVDecomposition<Real> svd;
if(!svd.set(Aeq)) {
if(solver.verbose>=1) cout<<"LP_InteriorPoint: Couldn't set SVD of equality constraints!!!"<<endl;
return false;
}
svd.backSub(beq,x0);
svd.getNullspace(N);
//Set the solver to use the new variable y
if(N.n == 0) { //overconstrained!
cout<<"Overconstrained!"<<endl;
solver.Resize(0,0);
return true;
}
if(nineq == 0) {
cout<<"No inequalities!"<<endl;
abort();
return true;
}
if(solver.verbose >= 1) cout<<"LP_InteriorPoint: Decomposed the problem from "<<lp.A.n<<" to "<<N.n<<" variables"<<endl;
solver.Resize(nineq,N.n);
//objective
foffset = dot(lp.c,x0);
//c is such that c'*y = lp.c'*N*y => c = N'*lp.c
N.mulTranspose(lp.c,solver.c);
solver.minimize = lp.minimize;
if(!solver.minimize) solver.c.inplaceNegative();
//inequality constraints
//q <= Aineq*x <= p
//q <= Aineq*x0 + Aineq*N*y <= p
//q - Aineq*x0 <= Aineq*N*y <= p-Aineq*x0
//==> -Aineq*N*y <= -q + Aineq*x0
nineq=0;
for(int i=0;i<lp.A.m;i++) {
if(lp.ConstraintType(i)==LinearProgram::Fixed) continue;
if(lp.HasUpperBound(lp.ConstraintType(i))) {
Vector Ai,sAi;
lp.A.getRowRef(i,Ai);
solver.A.getRowRef(nineq,sAi);
N.mulTranspose(Ai,sAi);
solver.p(nineq) = lp.p(i) - dot(Ai,x0);
nineq++;
}
if(lp.HasLowerBound(lp.ConstraintType(i))) {
Vector Ai,sAi;
lp.A.getRowRef(i,Ai);
solver.A.getRowRef(nineq,sAi);
N.mulTranspose(Ai,sAi);
sAi.inplaceNegative();
solver.p(nineq) = dot(Ai,x0) - lp.q(i);
nineq++;
}
}
//transform bounds to inequality constraints
for(int i=0;i<lp.u.n;i++) {
if(lp.VariableType(i)==LinearProgram::Fixed) continue;
if(lp.HasLowerBound(lp.VariableType(i))) {
//-xi < -li
//-ei'*N*y <= -li+ei'*x0
Vector Ni,sAi;
N.getRowRef(i,Ni);
solver.A.getRowRef(nineq,sAi);
sAi.setNegative(Ni);
solver.p(nineq) = -lp.l(i) + x0(i);
nineq++;
}
if(lp.HasUpperBound(lp.VariableType(i))) {
//xi < ui
//ei'*N*y <= ui-ei'*x0
Vector Ni,sAi;
N.getRowRef(i,Ni);
solver.A.getRowRef(nineq,sAi);
sAi.copy(Ni);
solver.p(nineq) = lp.u(i) - x0(i);
nineq++;
}
}
Assert(solver.IsValid());
return true;
}
