const typename
  Type<Scalar>::MatrixX& HumanoidLipComJerkMinimizationObjective<Scalar>::getHessian()
  {
    assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples());;

    int N = lipModel_.getNbSamples();
    int M = feetSupervisor_.getNbPreviewedSteps();

    int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1;

    const LinearDynamic<Scalar>& dynCopX = lipModel_.getCopXLinearDynamic(nb);
    const LinearDynamic<Scalar>& dynCopY = lipModel_.getCopYLinearDynamic(nb);

    MatrixX tmp = MatrixX::Zero(2*N, 2*N + 2*M);

    tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrixT();
    tmp.block(0, 2*N, N, M) = feetSupervisor_.getFeetPosLinearDynamic().U;
    tmp.block(N, 2*N + M, N, M) = feetSupervisor_.getFeetPosLinearDynamic().U;

    MatrixX tmp2 = MatrixX::Zero(2*N, 2*N);

    const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix();

    tmp2.block(0, 0, N, N) = dynCopX.UTinv*weight*dynCopX.Uinv;
    tmp2.block(N, N, N, N) = dynCopY.UTinv*weight*dynCopY.Uinv;
    hessian_.noalias() = tmp.transpose()*tmp2*tmp;

    return hessian_;
  }
Example #2
0
GMMExpectationMaximization::uint GMMExpectationMaximization::execute(const MatrixX & dataset)
{  
  const uint data_count = dataset.rows();
  const uint num_gaussians = m_means.size();
  const uint dim = dataset.cols();

  MatrixX pxi(data_count,num_gaussians);
  MatrixX pix(data_count,num_gaussians);
  VectorX pxidatatot(data_count);
  VectorX weights(num_gaussians);
  VectorX ex(data_count);
  MatrixX ts(dim,dim);
  VectorX dif(dim);

  Real prev_log_likelyhood = 1.0;
  
  uint it_num;
  for (it_num = 0; it_num < m_max_iterations; it_num++)
  {
    for (uint g = 0; g < num_gaussians; g++)
      weights[g] = m_weights[g];

    for (uint d = 0; d < data_count; d++)
      for (uint g = 0; g < num_gaussians; g++)
        pxi(d,g) = gauss(m_means[g],m_covs[g],dataset.row(d).transpose());

    pxidatatot = pxi * weights;
    Real log_likelyhood = pxidatatot.array().log().sum() / Real(data_count);

    if (it_num != 0 && (std::abs(log_likelyhood / prev_log_likelyhood - 1.0) < m_termination_threshold))
      break;
    prev_log_likelyhood = log_likelyhood;

    for (uint d = 0; d < data_count; d++)
      pix.row(d) = (pxi.row(d).transpose().array() * weights.array()).transpose() / pxidatatot[d];
    
    ex = pix.colwise().sum();

    for(uint g = 0; g < num_gaussians; g++)
    {
      m_weights[g] = ex[g] / Real(data_count);

      m_means[g] = (dataset.transpose() * pix.col(g)) / ex[g];

      ts = MatrixX::Zero(dim,dim);
      for (uint d = 0; d < data_count; d++)
      {
        dif = dataset.row(d).transpose() - m_means[g];
        ts.noalias() += (dif * dif.transpose()) * pix(d,g);
      }
      m_covs[g] = (ts / ex[g]) + MatrixX::Identity(dim,dim) * m_epsilon;
    }

    // interruption point here
    if (m_termination_handler && m_termination_handler->isTerminated())
      return it_num;
  }

  return it_num;
}