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;
}
Esempio n. 3
0
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());
}