void CPDNRigid<Scalar, Dim>::m_step()
{
Scalar N_P = this->P1_.sum();
if (!this->lr_)
{
TMatrix A = (this->P1_.asDiagonal()*G_ + paras_.lambda_*paras_.sigma2_*MatrixType<Scalar, Dim>::Matrix::Identity(this->M_, this->M_));
TMatrix B = this->PX_ - this->P1_.asDiagonal() * this->model_;
paras_.W_ = A.inverse() * B;
}
else
{
P1_correlation();
TMatrix A1 = ((1/(paras_.lambda_*paras_.sigma2_))*this->P1_).asDiagonal();
TMatrix A2 = Q_ * (S_.inverse() + Q_.transpose()*A1*Q_).inverse() * Q_.transpose();
TMatrix A_inv = A1 - A1 * A2 * A1;
TMatrix B = this->P1_.cwiseInverse().asDiagonal() * this->PX_ - this->model_;
paras_.W_ = A_inv * B;
}
align();
paras_.sigma2_ = 1/(N_P*Dim) * ((this->data_.transpose()*this->PT1_.asDiagonal()*this->data_).trace() -
2*(this->PX_.transpose()*this->T_).trace() + (this->T_.transpose()*this->P1_.asDiagonal()*this->T_).trace());
paras_.sigma2_ = fabs(paras_.sigma2_);
}
void CPDNRigid<T, D>::m_step()
{
T N_P = _P1.sum();
if (!_lr)
{
TMatrix A = (_P1.asDiagonal()*_G + _paras._lambda*_paras._sigma2*TMatrix::Identity(_M, _M));
TMatrix B = _PX - _P1.asDiagonal() * _model;
_paras._W = A.inverse() * B;
}
else
{
P1_correlation();
TMatrix A1 = ((1/(_paras._lambda*_paras._sigma2))*_P1).asDiagonal();
TMatrix A2 = _Q * (_S.inverse() + _Q.transpose()*A1*_Q).inverse() * _Q.transpose();
TMatrix A_inv = A1 - A1 * A2 * A1;
TMatrix B = _P1.cwiseInverse().asDiagonal() * _PX - _model;
_paras._W = A_inv * B;
}
align();
_paras._sigma2 = 1/(N_P*D) * ((_data.transpose()*_PT1.asDiagonal()*_data).trace() -
2*(_PX.transpose()*_T).trace() + (_T.transpose()*_P1.asDiagonal()*_T).trace());
_paras._sigma2 = abs(_paras._sigma2);
}