void TrajectoryWalkgen<Scalar>::setState(const VectorX& state)
{
assert(state==state);
assert(state.size()==3);
noDynModel_.setState(state);
}
void VelocityTrackingObjective<Scalar>::setVelRefInWorldFrame(const VectorX& velRefInWorldFrame)
{
assert(velRefInWorldFrame.size()==model_.getNbSamples());
assert(velRefInWorldFrame==velRefInWorldFrame);
velRefInWorldFrame_ = velRefInWorldFrame;
}
GMMExpectationMaximization::Real GMMExpectationMaximization::gauss(const VectorX & mean,
const MatrixX & cov,const VectorX & pt) const
{
Real det = cov.determinant();
uint dim = mean.size();
// check that the covariance matrix is invertible
if (std::abs(det) < std::pow(m_epsilon,dim) * 0.1)
return 0.0; // the gaussian has approximately zero width: the probability of any point falling into it is approximately 0.
// else, compute pdf
MatrixX inverse_cov = cov.inverse();
VectorX dist = pt - mean;
Real exp = - (dist.dot(inverse_cov * dist)) / 2.0;
Real den = std::sqrt(std::pow(2.0 * M_PI,dim) * std::abs(det));
return std::exp(exp) / den;
}
void TrajectoryWalkgen<Scalar>::setPosRefInWorldFrame(const VectorX& posRef)
{
assert(posRef==posRef);
assert(posRef.size()==noDynModel_.getNbSamples());
posTrackingObj_.setPosRefInWorldFrame(posRef);
}
void TrajectoryWalkgen<Scalar>::setVelRefInWorldFrame(const VectorX& velRef)
{
assert(velRef==velRef);
assert(velRef.size()==noDynModel_.getNbSamples());
velTrackingObj_.setVelRefInWorldFrame(velRef);
}
std::vector<double> Configurator::eigenToStdVector(const VectorX vec) {
return std::vector<double>(vec.data(), vec.data() + vec.size());
}