//************************************************************************************************** VectorXd wholeBodyReach::svdSolveWithDamping(const JacobiSVD<MatrixRXd>& A, VectorConst b, double damping) { eigen_assert(A.computeU() && A.computeV() && "svdSolveWithDamping() requires both unitaries U and V to be computed (thin unitaries suffice)."); assert(A.rows()==b.size()); // cout<<"b = "<<toString(b,1)<<endl; VectorXd tmp(A.cols()); int nzsv = A.nonzeroSingularValues(); tmp.noalias() = A.matrixU().leftCols(nzsv).adjoint() * b; // cout<<"U^T*b = "<<toString(tmp,1)<<endl; double sv, d2 = damping*damping; for(int i=0; i<nzsv; i++) { sv = A.singularValues()(i); tmp(i) *= sv/(sv*sv + d2); } // cout<<"S^+ U^T b = "<<toString(tmp,1)<<endl; VectorXd res = A.matrixV().leftCols(nzsv) * tmp; // getLogger().sendMsg("sing val = "+toString(A.singularValues(),3), MSG_STREAM_INFO); // getLogger().sendMsg("solution with damp "+toString(damping)+" = "+toString(res.norm()), MSG_STREAM_INFO); // getLogger().sendMsg("solution without damping ="+toString(A.solve(b).norm()), MSG_STREAM_INFO); return res; }
VectorXd svdSolve( JacobiSVD<MatrixXd> & svd,VectorXd & ref, double * rankPtr = NULL, double EPS=1e-6 ) { typedef JacobiSVD<MatrixXd> SVDType; const JacobiSVD<MatrixXd>::MatrixUType &U = svd.matrixU(); const JacobiSVD<MatrixXd>::MatrixVType &V = svd.matrixV(); const JacobiSVD<MatrixXd>::SingularValuesType &s = svd.singularValues(); //std::cout << (MATLAB)U<< (MATLAB)s<<(MATLAB)V << std::endl; const int N=V.rows(),M=U.rows(),S=std::min(M,N); int rank=0; while( rank<S && s[rank]>EPS ) rank++; VectorXd sinv = s.head(rank).array().inverse(); //std::cout << "U = " << (MATLAB)U.leftCols(rank) << std::endl; //std::cout << "V = " << (MATLAB)V.leftCols(rank) << std::endl; //std::cout << "S = " << (MATLAB)(MatrixXd)sinv.asDiagonal() << std::endl; VectorXd x; x = V.leftCols(rank) * sinv.asDiagonal() * U.leftCols(rank).transpose() * ref; if(rankPtr!=NULL) *rankPtr = rank; return x; }
void dampedInverse( const JacobiSVD <dg::Matrix>& svd, dg::Matrix& _inverseMatrix, const double threshold) { typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t; ArrayWrapper<const SV_t> sigmas (svd.singularValues()); SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold)); const dg::Matrix::Index m = sv_inv.size(); _inverseMatrix.noalias() = ( svd.matrixV().leftCols(m) * sv_inv.asDiagonal() * svd.matrixU().leftCols(m).transpose()); }
VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) { // const int MAX_SAMPLE = 20000; const bool fast = true, very_fast = true; // Remove the DC (Y : N x M) RMatrixXf dY = Y.rowwise() - Y.colwise().mean(); // RMatrixXf sY = dY; // if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) { // VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE ); // std::sort( samples.data(), samples.data()+samples.size() ); // sY = RMatrixXf( samples.size(), dY.cols() ); // for( int i=0; i<samples.size(); i++ ) // sY.row(i) = dY.row( samples[i] ); // } // ... and use (pc > 0) VectorXf lbl = VectorXf::Zero( Y.rows() ); // Find the largest PC of (dY.T * dY) and project onto it if( very_fast ) { // Find the largest PC using poweriterations VectorXf U = VectorXf::Random( dY.cols() ); U = U.array() / U.norm()+std::numeric_limits<float>::min(); for( int it=0; it<20; it++ ){ // Normalize VectorXf s = dY.transpose()*(dY*U); s.array() /= s.norm()+std::numeric_limits<float>::min(); if ( (U-s).norm() < 1e-6 ) break; U = s; } // Project onto the PC lbl = dY*U; } else if(fast) { // Compute the eigen values of the covariance (and project onto the largest eigenvector) MatrixXf cov = dY.transpose()*dY; SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose())); MatrixXf ev = eigensolver.eigenvectors(); lbl = dY * ev.col( ev.cols()-1 ); } else { // Use the SVD JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV ); // Project onto the largest PC lbl = svd.matrixU().col(0) * svd.singularValues()[0]; } // Find the representative label if( rep_label ) dY.array().square().rowwise().sum().minCoeff( rep_label ); return (lbl.array() < 0).cast<float>(); }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { using namespace Eigen; if(nrhs < 1 || nlhs != 1) { mexWarnMsgTxt("Minimum one input and one output parameter required"); return; } //Parse opts bool fast2D = false; if(nrhs > 1) { const mxArray *opt = prhs[1]; const mxArray *opt_fast2D = mxGetField(opt, 0, "fast2D"); if(opt_fast2D) fast2D = mxGetScalar(opt_fast2D); } const mwSize* dims = mxGetDimensions(prhs[0]); const mwSize m = dims[0], n = dims[1]; if(n%m != 0) mexErrMsgTxt("Invalid input (inconsistent dimensions)."); mxArray *R = mxCreateNumericMatrix(m, n, mxDOUBLE_CLASS, mxREAL); const double *data_in = mxGetPr(prhs[0]); double *data_out = mxGetPr(R); const mwSize nt = n / m; if(m == 2) { if(fast2D) { //special case closed form solution for 2x2 for(mwSize t = 0; t < nt; ++t) { //get transformation Map<const Matrix2d> T(data_in + 4*t); //closed form solution double trace = T.trace(); double offdiff = T(0,1) - T(1,0); double off_term = offdiff / trace; double denom_global = 1 / sqrt(off_term*off_term + 1); Matrix2d R; R(0,0) = denom_global; R(0,1) = off_term * denom_global; R(1,0) = -R(0,1); R(1,1) = R(0,0); if(R.determinant() < 0) //safety checks R *= 1; Map<Matrix2d>(data_out + 4*t) = R; } } else { //hope for better compiler optimization for(mwSize t = 0; t < nt; ++t) { //get transformation Map<const Matrix2d> T(data_in + 4*t); //get svd const JacobiSVD<Matrix2d> svd = T.jacobiSvd(ComputeFullU | ComputeFullV); Matrix2d R(svd.matrixU() * svd.matrixV().transpose()); if(svd.singularValues().prod() < 0) //safety checks R *= -1; Map<Matrix2d>(data_out + 4*t) = R; } } } else if(m == 3) { //hope for better compiler optimization for(mwSize t = 0; t < nt; ++t) { //get transformation Map<const Matrix3d> T(data_in + 9*t); //get svd const JacobiSVD<Matrix3d> svd = T.jacobiSvd(ComputeFullU | ComputeFullV); Matrix3d R(svd.matrixU() * svd.matrixV().transpose()); if(svd.singularValues().prod() < 0) //safety checks R *= -1; Map<Matrix3d>(data_out + 9*t) = R; } } else { //generic nd implementation for(mwSize t = 0; t < nt; ++t) { //get transformation Map<const MatrixXd> T(data_in + m*m*t, m, m); //get svd const JacobiSVD<MatrixXd> svd = T.jacobiSvd(ComputeFullU | ComputeFullV); MatrixXd R(svd.matrixU() * svd.matrixV().transpose()); if(svd.singularValues().prod() < 0) //safety checks R *= -1; Map<MatrixXd>(data_out + m*m*t, m, m) = R; } } plhs[0] = R; }
// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html ) void pinv(const MatrixXd &b, MatrixXd &a_pinv) { // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method // TODO: Figure out why it wants fewer rows than columns // if ( a.rows()<a.cols() ) // return false; bool flip = false; MatrixXd a; if( a.rows() < a.cols() ) { a = b.transpose(); flip = true; } else a = b; // SVD JacobiSVD< MatrixXd > svdA; svdA.compute(a, ComputeFullU | ComputeThinV); JacobiSVD<MatrixXd>::SingularValuesType vSingular = svdA.singularValues(); // Build a diagonal matrix with the Inverted Singular values // The pseudo inverted singular matrix is easy to compute : // is formed by replacing every nonzero entry by its reciprocal (inversing). VectorXd vPseudoInvertedSingular(svdA.matrixV().cols()); for (int iRow =0; iRow<vSingular.rows(); iRow++) { if ( fabs(vSingular(iRow))<=1e-10 ) // Todo : Put epsilon in parameter { vPseudoInvertedSingular(iRow)=0.; } else { vPseudoInvertedSingular(iRow)=1./vSingular(iRow); } } // A little optimization here MatrixXd mAdjointU = svdA.matrixU().adjoint().block(0,0,vSingular.rows(),svdA.matrixU().adjoint().cols()); // Pseudo-Inversion : V * S * U' a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU; if(flip) { a = a.transpose(); a_pinv = a_pinv.transpose(); } }
// Based on a paper by Samuel R. Buss and Jin-Su Kim // TODO: Cite the paper properly rk_result_t Robot::selectivelyDampedLeastSquaresIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues, const Isometry3d &target, const Isometry3d &finalTF) { return RK_SOLVER_NOT_READY; // FIXME: Make this work // Arbitrary constant for maximum angle change in one step gammaMax = M_PI/4; // TODO: Put this in the constructor so the user can change it at a whim vector<Linkage::Joint*> joints; joints.resize(jointIndices.size()); // FIXME: Add in safety checks for(int i=0; i<joints.size(); i++) joints[i] = joints_[jointIndices[i]]; // ~~ Declarations ~~ MatrixXd J; JacobiSVD<MatrixXd> svd; Isometry3d pose; AngleAxisd aagoal; AngleAxisd aastate; Vector6d goal; Vector6d state; Vector6d err; Vector6d alpha; Vector6d N; Vector6d M; Vector6d gamma; VectorXd delta(jointValues.size()); VectorXd tempPhi(jointValues.size()); // ~~~~~~~~~~~~~~~~~~ // cout << "\n\n" << endl; tolerance = 1*M_PI/180; // TODO: Put this in the constructor so the user can set it arbitrarily maxIterations = 1000; // TODO: Put this in the constructor so the user can set it arbitrarily size_t iterations = 0; do { values(jointIndices, jointValues); jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this); svd.compute(J, ComputeFullU | ComputeThinV); // cout << "\n\n" << svd.matrixU() << "\n\n\n" << svd.singularValues().transpose() << "\n\n\n" << svd.matrixV() << endl; // for(int i=0; i<svd.matrixU().cols(); i++) // cout << "u" << i << " : " << svd.matrixU().col(i).transpose() << endl; // std::cout << "Joint name: " << joint(jointIndices.back()).name() // << "\t Number: " << jointIndices.back() << std::endl; pose = joint(jointIndices.back()).respectToRobot()*finalTF; // std::cout << "Pose: " << std::endl; // std::cout << pose.matrix() << std::endl; // AngleAxisd aagoal(target.rotation()); aagoal = target.rotation(); goal << target.translation(), aagoal.axis()*aagoal.angle(); aastate = pose.rotation(); state << pose.translation(), aastate.axis()*aastate.angle(); err = goal-state; // std::cout << "state: " << state.transpose() << std::endl; // std::cout << "err: " << err.transpose() << std::endl; for(int i=0; i<6; i++) alpha[i] = svd.matrixU().col(i).dot(err); // std::cout << "Alpha: " << alpha.transpose() << std::endl; for(int i=0; i<6; i++) { N[i] = svd.matrixU().block(0,i,3,1).norm(); N[i] += svd.matrixU().block(3,i,3,1).norm(); } // std::cout << "N: " << N.transpose() << std::endl; double tempMik = 0; for(int i=0; i<svd.matrixV().cols(); i++) { M[i] = 0; for(int k=0; k<svd.matrixU().cols(); k++) { tempMik = 0; for(int j=0; j<svd.matrixV().cols(); j++) tempMik += fabs(svd.matrixV()(j,i))*J(k,j); M[i] += 1/svd.singularValues()[i]*tempMik; } } // std::cout << "M: " << M.transpose() << std::endl; for(int i=0; i<svd.matrixV().cols(); i++) gamma[i] = minimum(1, N[i]/M[i])*gammaMax; // std::cout << "Gamma: " << gamma.transpose() << std::endl; delta.setZero(); for(int i=0; i<svd.matrixV().cols(); i++) { // std::cout << "1/sigma: " << 1/svd.singularValues()[i] << std::endl; tempPhi = 1/svd.singularValues()[i]*alpha[i]*svd.matrixV().col(i); // std::cout << "Phi: " << tempPhi.transpose() << std::endl; clampMaxAbs(tempPhi, gamma[i]); delta += tempPhi; // std::cout << "delta " << i << ": " << delta.transpose() << std::endl; } clampMaxAbs(delta, gammaMax); jointValues += delta; std::cout << iterations << " | Norm:" << delta.norm() << "\tdelta: " << delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl; iterations++; } while(delta.norm() > tolerance && iterations < maxIterations); }