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_;
}
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;
}
