typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPointErrorMinimizer::compute(
const DataPoints& filteredReading,
const DataPoints& filteredReference,
const OutlierWeights& outlierWeights,
const Matches& matches)
{
assert(matches.ids.rows() > 0);
typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
// now minimize on kept points
const int dimCount(mPts.reading.features.rows());
const int ptsCount(mPts.reading.features.cols()); //Both point clouds have now the same number of (matched) point
// Compute the (weighted) mean of each point cloud
const Vector& w = mPts.weights;
const T w_sum_inv = T(1.)/w.sum();
const Vector meanReading =
(mPts.reading.features.topRows(dimCount-1).array().rowwise() * w.array().transpose()).rowwise().sum() * w_sum_inv;
const Vector meanReference =
(mPts.reference.features.topRows(dimCount-1).array().rowwise() * w.array().transpose()).rowwise().sum() * w_sum_inv;
// Remove the mean from the point clouds
mPts.reading.features.topRows(dimCount-1).colwise() -= meanReading;
mPts.reference.features.topRows(dimCount-1).colwise() -= meanReference;
// Singular Value Decomposition
const Matrix m(mPts.reference.features.topRows(dimCount-1) * w.asDiagonal()
* mPts.reading.features.topRows(dimCount-1).transpose());
const JacobiSVD<Matrix> svd(m, ComputeThinU | ComputeThinV);
Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
// It is possible to get a reflection instead of a rotation. In this case, we
// take the second best solution, guaranteed to be a rotation. For more details,
// read the tech report: "Least-Squares Rigid Motion Using SVD", Olga Sorkine
// http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
if (rotMatrix.determinant() < 0.)
{
Matrix tmpV = svd.matrixV().transpose();
tmpV.row(dimCount-2) *= -1.;
rotMatrix = svd.matrixU() * tmpV;
}
const Vector trVector(meanReference - rotMatrix * meanReading);
Matrix result(Matrix::Identity(dimCount, dimCount));
result.topLeftCorner(dimCount-1, dimCount-1) = rotMatrix;
result.topRightCorner(dimCount-1, 1) = trVector;
return result;
}
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;
}
//**************************************************************************************************
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;
}
typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPointErrorMinimizer::compute(
const DataPoints& filteredReading,
const DataPoints& filteredReference,
const OutlierWeights& outlierWeights,
const Matches& matches)
{
assert(matches.ids.rows() > 0);
typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
// now minimize on kept points
const int dimCount(mPts.reading.features.rows());
const int ptsCount(mPts.reading.features.cols()); //But point cloud have now the same number of (matched) point
// Compute the mean of each point cloud
const Vector meanReading = mPts.reading.features.rowwise().sum() / ptsCount;
const Vector meanReference = mPts.reference.features.rowwise().sum() / ptsCount;
// Remove the mean from the point clouds
mPts.reading.features.colwise() -= meanReading;
mPts.reference.features.colwise() -= meanReference;
// Singular Value Decomposition
const Matrix m(mPts.reference.features.topRows(dimCount-1) * mPts.reading.features.topRows(dimCount-1).transpose());
const JacobiSVD<Matrix> svd(m, ComputeThinU | ComputeThinV);
const Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
const Vector trVector(meanReference.head(dimCount-1)- rotMatrix * meanReading.head(dimCount-1));
Matrix result(Matrix::Identity(dimCount, dimCount));
result.topLeftCorner(dimCount-1, dimCount-1) = rotMatrix;
result.topRightCorner(dimCount-1, 1) = trVector;
return result;
}
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());
}
void CVMath::setupMatrixM2()
{
Line r1 = Points::getInstance().getR1();
Line r2 = Points::getInstance().getR2();
Line r3 = Points::getInstance().getR3();
Line r4 = Points::getInstance().getR4();
l0 << r1.x, r1.y, r1.z;
m0 << r2.x, r2.y, r2.z;
l1 << r3.x, r3.y, r3.z;
m1 << r4.x, r4.y, r4.z;
MatrixXd LT_M = MatrixXd(2, 3);
LT_M << l0(0)*m0(0), l0(0)*m0(1) + l0(1)*m0(0), l0(1)*m0(1),
l1(0)*m1(0), l1(0)*m1(1) + l1(1)*m1(0), l1(1)*m1(1);
JacobiSVD<MatrixXd> svdA (LT_M, Eigen::ComputeFullV);
VectorXd x = svdA.matrixV().col(2);
MatrixXd KKT = MatrixXd(2, 2);
KKT << x(0), x(1),
x(1), x(2);
MatrixXd HHT = MatrixXd(3, 3);
HHT << x(0), x(1), 0,
x(1), x(2), 0,
0, 0, 0;
MatrixXd aux = MatrixXd(2, 2);
aux = HHT.llt().matrixU();
std::cout << x << std::endl;
std::cout << aux << std::endl;
Hp = MatrixXd(3, 3);
//add 1 ao final para se tornar inversivel
Hp << aux(0,0), aux(0,1), 0,
0, aux(1, 1), 0,
0, 0, 1;
std::cout << Hp << std::endl;
Hp_INV = MatrixXd(3, 3);
Hp_INV = Hp.inverse().eval();
std::cout << Hp_INV << std::endl;
}
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);
}
