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