Scalar CPDNRigid<Scalar, Dim>::energy()
{
Scalar e = 0;
for (size_t n = 0; n < this->N_; n ++)
{
Scalar sp = 0;
for (size_t m = 0; m < this->M_; m ++)
{
sp += computeGaussianExp(m, n);
}
sp += pow((2*M_PI*paras_.sigma2_), 0.5*Dim) * (this->w_/(1-this->w_)) * (Scalar(this->M_)/this->N_);
e += -log(sp);
}
e += this->N_ * Dim * log(paras_.sigma2_) / 2;
return e;
}
T CPDRigid<T, D>::energy()
{
T e = 0;
for (size_t n = 0; n < _N; n ++)
{
T sp = 0;
for (size_t m = 0; m < _M; m ++)
{
sp += computeGaussianExp(m, n);
}
sp += pow((2*M_PI*_paras._sigma2), 0.5*D) * (_w/(1-_w)) * (T(_M)/_N);
e += -log(sp);
}
e += _N * D * log(_paras._sigma2) / 2;
return e;
}
void CPDNRigid<Scalar, Dim>::correspondences()
{
this->corres_.setZero(this->M_, this->N_);
for (size_t n = 0; n < this->N_; n ++)
{
typename std::vector<Scalar> t_exp;
Scalar sum_exp = 0;
Scalar c = pow((2*M_PI*paras_.sigma2_), 0.5*Dim) * (this->w_/(1-this->w_)) * (Scalar(this->M_)/this->N_);
for (size_t m = 0; m < this->M_; m ++)
{
Scalar m_exp = computeGaussianExp(m, n);
t_exp.push_back(m_exp);
sum_exp += m_exp;
}
for (size_t m = 0; m < this->M_; m ++)
{
this->corres_(m, n) = t_exp.at(m) / (sum_exp + c);
}
}
}
void CPDRigid<T, D>::correspondences()
{
_corres.setZero(_M, _N);
for (size_t n = 0; n < _N; n ++)
{
typename std::vector<T> t_exp;
T sum_exp = 0;
T c = pow((2*M_PI*_paras._sigma2), 0.5*D) * (_w/(1-_w)) * (T(_M)/_N);
for (size_t m = 0; m < _M; m ++)
{
T m_exp = computeGaussianExp(m, n);
t_exp.push_back(m_exp);
sum_exp += m_exp;
}
for (size_t m = 0; m < _M; m ++)
{
_corres(m, n) = t_exp.at(m) / (sum_exp + c);
}
}
}