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::setTangentDimension(Index td)
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(td >= 0 && "Negative dimension not accepted");
testLock();
tangentDim_ = td;
}
void Manifold::setRepresentationDimension(Index rd)
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(rd >= 0 && "Negative dimension not accepted");
testLock();
representationDim_ = rd;
}
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::setDimension(Index d)
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(d >= 0 && "Negative dimension not accepted");
testLock();
dimension_ = d;
}
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::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 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::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 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);
}
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);
}
Manifold::Manifold(Index dimension, Index tangentDimension,
Index representationDimension)
: dimension_(dimension),
tangentDim_(tangentDimension),
representationDim_(representationDimension),
lock_(false)
{
mnf_assert(0 <= dimension && "Negative dimension not accepted");
mnf_assert(dimension <= tangentDimension);
mnf_assert(tangentDimension <= representationDimension);
this->instanceId_ = manifoldCounter_++;
}
Point Manifold::getZero() const
{
mnf_assert(isValid() || seeMessageAbove());
lock();
Eigen::VectorXd id(representationDim_);
setZero(id);
return Point(*this, id);
}
Point Manifold::createRandomPoint(double coeff) const
{
mnf_assert(isValid() || seeMessageAbove());
lock();
Eigen::VectorXd val(representationDim_);
createRandomPoint(val, coeff);
return Point(*this, val);
}
void ExpMapQuaternion::applyDiffPseudoLog0_(
RefMat out, const ConstRefMat& in, const ConstRefVec& x, ReusableTemporaryMap& m)
{
mnf_assert(in.cols() == InputDim_ && "Dimensions mismatch" );
Eigen::Map<Eigen::MatrixXd, Eigen::Aligned> a = m.getMap(in.rows(),OutputDim_);
a.noalias() = in*diffPseudoLog0_(x);
out = a;
}
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;
}
Point Manifold::createPoint(const ConstRefVec& val) const
{
mnf_assert(isValid() || seeMessageAbove());
if (isInM(val))
{
lock();
return Point(*this, val);
}
else
{
throw std::runtime_error("Bad Point Initialization");
}
}
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);
}
Index Manifold::dim() const
{
mnf_assert(isValid() || seeMessageAbove());
return dimension_;
}
Index Manifold::representationDim() const
{
mnf_assert(isValid() || seeMessageAbove());
return representationDim_;
}
void Manifold::createRandomPoint(RefVec out, double coeff) const
{
mnf_assert(out.size() == representationDim_ &&
"wrong dimension in Manifold::createRandomPoint");
createRandomPoint_(out, coeff);
}
void Manifold::pseudoLog0(RefVec out, const ConstRefVec& x) const
{
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim_);
pseudoLog0_(out, x);
}
Eigen::MatrixXd Manifold::diffPseudoLog0(const ConstRefVec& x) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(x.size() == representationDim_);
return diffPseudoLog0_(x);
}
bool Manifold::isInM(const Eigen::VectorXd& val, double prec) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(val.size() == representationDim());
return isInM_(val, prec);
}
Point Manifold::createPoint() const
{
mnf_assert(isValid() || seeMessageAbove());
lock();
return Point(*this);
}
void Manifold::getTrustMagnitude(RefVec out) const
{
mnf_assert(out.size() == tangentDim_);
getTrustMagnitude_(out);
}
void Manifold::limitMap(RefVec out) const
{
mnf_assert(out.size() == tangentDim_);
limitMap_(out);
}
void Manifold::setZero(RefVec out) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == representationDim_);
setZero_(out);
}
Index Manifold::tangentDim() const
{
mnf_assert(isValid() || seeMessageAbove());
return tangentDim_;
}
