bool Manifold::isInTxM(const ConstRefVec& x, const ConstRefVec& v,
const double& prec) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(v.size() == tangentDim_);
mnf_assert(x.size() == representationDim());
return isInTxM_(x, v, prec);
}
void Manifold::pseudoLog(RefVec out, const ConstRefVec& x,
const ConstRefVec& y) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim_);
mnf_assert(y.size() == representationDim_);
pseudoLog_(out, x, y);
}
void ExpMapQuaternion::pseudoLog_(RefVec out, const ConstRefVec& x, const ConstRefVec& y)
{
Eigen::Vector4d tmp;
toQuat q(tmp.data());
const toConstQuat xQ(x.data());
const toConstQuat yQ(y.data());
q = xQ.inverse()*yQ; //TODO double-check that formula
logarithm(out,tmp);
}
void Manifold::forceOnTxM(RefVec out, const ConstRefVec& in,
const ConstRefVec& x) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim());
mnf_assert(in.size() == tangentDim_);
forceOnTxM_(out, in, x);
}
void Manifold::retractation(RefVec out, const ConstRefVec& x,
const ConstRefVec& v) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == representationDim_);
mnf_assert(x.size() == representationDim_);
mnf_assert(v.size() == tangentDim_);
mnf_assert(isInTxM(x, v) && "Wrong tangent vector provided to retractation");
retractation_(out, x, v);
}
void Manifold::applyTransport(RefMat out, const ConstRefMat& in,
const ConstRefVec& x, const ConstRefVec& v) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(in.rows() == tangentDim_);
mnf_assert(out.rows() == tangentDim_);
mnf_assert(in.cols() == out.cols());
mnf_assert(x.size() == representationDim());
mnf_assert(v.size() == tangentDim_);
mnf_assert(isInTxM(x, v));
applyTransport_(out, in, x, v);
}
void ExpMapQuaternion::forceOnM_(RefVec out, const ConstRefVec& in)
{
toConstQuat inQuat(in.data());
toQuat outQuat(out.data());
outQuat = inQuat;
out.normalize();
}
void Manifold::forceOnM(RefVec out, const ConstRefVec& in) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == representationDim());
mnf_assert(in.size() == representationDim());
return forceOnM_(out, in);
}
void Manifold::getIdentityOnTxM(RefMat out, const ConstRefVec& x) const
{
mnf_assert(out.rows() == tangentDim_);
mnf_assert(out.cols() == tangentDim_);
mnf_assert(x.size() == representationDim());
getIdentityOnTxM_(out, x);
}
void Manifold::tangentConstraint(RefMat out, const ConstRefVec& x) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.rows() == tangentDim_ - dimension_);
mnf_assert(out.cols() == tangentDim_);
mnf_assert(x.size() == representationDim());
tangentConstraint_(out, x);
}
void ExpMapQuaternion::exponential(OutputType& q, const ConstRefVec& v)
{
mnf_assert(v.size() == 3 && "Increment for expMap must be of size 3");
double n2 = v.squaredNorm(); // (theta)^2 (in Grassia)
mnf_assert(sqrt(n2) < M_PI && "Increment for expMap must be of norm at most pi");
double s; // sin(theta/2)/theta in Grassia
if (n2 < prec)
{
toQuat(q.data()).w() = 1 + (-1 + n2 / 48)*(n2/8);// cos(theta/2) in Grassia
s = (1+(-1+0.0125*n2)*n2/24)/2;
}
else
{
double t = sqrt(n2); // theta (in Grassia)
toQuat(q.data()).w() = cos(0.5*t);
s = sin(0.5*t) / t;
}
toQuat(q.data()).vec() = s*v;
}
Eigen::Matrix<double, 4, 3> ExpMapQuaternion::diffRetractation_(const ConstRefVec& x)
{
const Eigen::Map<const Eigen::Quaterniond> xQ(x.data());
Eigen::Matrix<double, 4, 3> J;
J << 0.5*xQ.w(), -0.5*xQ.z(), 0.5*xQ.y(),
0.5*xQ.z(), 0.5*xQ.w(), -0.5*xQ.x(),
-0.5*xQ.y(), 0.5*xQ.x(), 0.5*xQ.w(),
-0.5*xQ.x(), -0.5*xQ.y(), -0.5*xQ.z();
return J;
}
void Manifold::applyDiffPseudoLog0(RefMat out, const ConstRefMat& in,
const ConstRefVec& x) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.cols() == representationDim_);
mnf_assert(in.cols() == tangentDim_);
mnf_assert(in.rows() == out.rows());
mnf_assert(x.size() == representationDim_);
applyDiffPseudoLog0_(out, in, x);
}
Eigen::Matrix<double, 3, 4> ExpMapQuaternion::diffPseudoLog0_(const ConstRefVec& v)
{
const toConstQuat vQ(v.data());
double n2 = vQ.vec().squaredNorm();
double n = sqrt(n2);
Eigen::Matrix<double, 3, 4> J;
if (n < prec && vQ.w()!=0 )
{
double a = 2/vQ.w();
double b = -2/(vQ.w()*vQ.w());
J << a, 0, 0, b*vQ.x(),
0, a, 0, b*vQ.y(),
0, 0, a, b*vQ.z();
}
else
{
// log(x,y,z,w) = f(x,y,z,w)*[x;y;z]
double f = atan2(2 * n * vQ.w(), vQ.w() * vQ.w() - n2) / n;
// df/dx = (x*atan((2*w*(x^2 + y^2 + z^2)^(1/2))/(- w^2 + x^2 + y^2 + z^2)))/(x^2 + y^2 + z^2)^(3/2) - ((2*w*x)/((x^2 + y^2 + z^2)^(1/2)*(- w^2 + x^2 + y^2 + z^2)) - (4*w*x*(x^2 + y^2 + z^2)^(1/2))/(- w^2 + x^2 + y^2 + z^2)^2)/(((4*w^2*(x^2 + y^2 + z^2))/(- w^2 + x^2 + y^2 + z^2)^2 + 1)*(x^2 + y^2 + z^2)^(1/2))
// df/dy = (y*atan((2*w*(x^2 + y^2 + z^2)^(1/2))/(- w^2 + x^2 + y^2 + z^2)))/(x^2 + y^2 + z^2)^(3/2) - ((2*w*y)/((x^2 + y^2 + z^2)^(1/2)*(- w^2 + x^2 + y^2 + z^2)) - (4*w*y*(x^2 + y^2 + z^2)^(1/2))/(- w^2 + x^2 + y^2 + z^2)^2)/(((4*w^2*(x^2 + y^2 + z^2))/(- w^2 + x^2 + y^2 + z^2)^2 + 1)*(x^2 + y^2 + z^2)^(1/2))
// df/dz = (z*atan((2*w*(x^2 + y^2 + z^2)^(1/2))/(- w^2 + x^2 + y^2 + z^2)))/(x^2 + y^2 + z^2)^(3/2) - ((2*w*z)/((x^2 + y^2 + z^2)^(1/2)*(- w^2 + x^2 + y^2 + z^2)) - (4*w*z*(x^2 + y^2 + z^2)^(1/2))/(- w^2 + x^2 + y^2 + z^2)^2)/(((4*w^2*(x^2 + y^2 + z^2))/(- w^2 + x^2 + y^2 + z^2)^2 + 1)*(x^2 + y^2 + z^2)^(1/2))
// g = (atan((2*w*n)/(- w^2 + n2)))/(n2)^(3/2) - ((2*w)/(n*(- w^2 + n2)) - (4*w*n)/(- w^2 + n2)^2)/(((4*w^2*n2)/(- w^2 + n2)^2 + 1)*n)
// g = (atan((2*w*n)/(- w^2 + n2)))/(n2*n) - ((2*w/n2)*(- w^2 + n2) - 4*w)/(4*w^2*n2+(- w^2 + n2)^2)
// df/dx = g*x = (x*atan((2*w*n)/(- w^2 + n2)))/(n2)^(3/2) - ((2*w*x)/(n*(- w^2 + n2)) - (4*w*x*n)/(- w^2 + n2)^2)/(((4*w^2*n2)/(- w^2 + n2)^2 + 1)*n)
// df/dy = g*y = (y*atan((2*w*n)/(- w^2 + n2)))/(n2)^(3/2) - ((2*w*y)/(n*(- w^2 + n2)) - (4*w*y*n)/(- w^2 + n2)^2)/(((4*w^2*n2)/(- w^2 + n2)^2 + 1)*n)
// df/dz = g*z = (z*atan((2*w*n)/(- w^2 + n2)))/(n2)^(3/2) - ((2*w*z)/(n*(- w^2 + n2)) - (4*w*z*n)/(- w^2 + n2)^2)/(((4*w^2*n2)/(- w^2 + n2)^2 + 1)*n)
// df/dw = -2/(w²+x²+y²+z²)
double g = (atan((2*vQ.w()*n)/(- vQ.w()*vQ.w() + n2)))/(n2*n) - ((2*vQ.w()/n2)*(- vQ.w()*vQ.w() + n2) - 4*vQ.w())/(4*vQ.w()*vQ.w()*n2+(- vQ.w()*vQ.w() + n2)*(- vQ.w()*vQ.w() + n2));
double dfdw = -2/(vQ.w()*vQ.w() + n2);
/*
* J = [ g.x²+f, g.y.x, g.z.x, df/dw.x]
* [ g.x.y, g.y²+f, g.z.y, df/dw.y]
* [ g.x.z, g.y.z, g.z²+f, df/dw.z]
*/
J << g*vQ.x()*vQ.x()+f, g*vQ.y()*vQ.x(), g*vQ.z()*vQ.x(), dfdw*vQ.x(),
g*vQ.x()*vQ.y(), g*vQ.y()*vQ.y()+f, g*vQ.z()*vQ.y(), dfdw*vQ.y(),
g*vQ.x()*vQ.z(), g*vQ.y()*vQ.z(), g*vQ.z()*vQ.z()+f, dfdw*vQ.z();
}
return J;
}
void Manifold::pseudoLog0(RefVec out, const ConstRefVec& x) const
{
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim_);
pseudoLog0_(out, x);
}
void ExpMapQuaternion::retractation_(RefVec out, const ConstRefVec& x, const ConstRefVec& v)
{
OutputType q;
exponential(q,v);
toQuat(out.data()) = (toConstQuat(x.data()))*(toConstQuat(q.data())); //out = x*exp(v)
}
Eigen::MatrixXd Manifold::diffPseudoLog0(const ConstRefVec& x) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(x.size() == representationDim_);
return diffPseudoLog0_(x);
}
