示例#1
0
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;
    */
  }
}
示例#2
0
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;
}
示例#3
0
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;
}
示例#4
0
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;
}
示例#5
0
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;
}
示例#6
0
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;
}
示例#7
0
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;
}
示例#8
0
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;
}
示例#9
0
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;
}
示例#10
0
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;
}
示例#11
0
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;
}