void Aligner::_computeStatistics(Vector6f &mean, Matrix6f &Omega,
float &translationalRatio, float &rotationalRatio) const {
typedef SigmaPoint<Vector6f> SigmaPoint;
typedef std::vector<SigmaPoint, Eigen::aligned_allocator<SigmaPoint> > SigmaPointVector;
// Output init
Vector6f b;
Matrix6f H;
b.setZero();
H.setZero();
translationalRatio = std::numeric_limits<float>::max();
rotationalRatio = std::numeric_limits<float>::max();
Eigen::Isometry3f invT = _T.inverse();
invT.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
_linearizer->setT(invT);
_linearizer->update();
H += _linearizer->H() + Matrix6f::Identity();
b += _linearizer->b();
JacobiSVD<Matrix6f> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
Matrix6f localSigma = svd.solve(Matrix6f::Identity());
SigmaPointVector sigmaPoints;
Vector6f localMean = Vector6f::Zero();
sampleUnscented(sigmaPoints, localMean, localSigma);
Eigen::Isometry3f dT = _T; // Transform from current to reference
// Remap each of the sigma points to their original position
//#pragma omp parallel
for (size_t i = 0; i < sigmaPoints.size(); i++) {
SigmaPoint &p = sigmaPoints[i];
p._sample = t2v(dT * v2t(p._sample).inverse());
}
// Reconstruct the gaussian
reconstructGaussian(mean, localSigma, sigmaPoints);
// Compute the information matrix from the covariance
Omega = localSigma.inverse();
// Have a look at the svd of the rotational and the translational part;
JacobiSVD<Matrix3f> partialSVD;
partialSVD.compute(Omega.block<3, 3>(0, 0));
translationalRatio = partialSVD.singularValues()(0) / partialSVD.singularValues()(2);
partialSVD.compute(Omega.block<3, 3>(3, 3));
rotationalRatio = partialSVD.singularValues()(0) / partialSVD.singularValues()(2);
}
// 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);
}
