本文整理汇总了C++中vpMatrix::AtA方法的典型用法代码示例。如果您正苦于以下问题：C++ vpMatrix::AtA方法的具体用法？C++ vpMatrix::AtA怎么用？C++ vpMatrix::AtA使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类vpMatrix的用法示例。
在下文中一共展示了vpMatrix::AtA方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: LMA
void
vpMbKltTracker::computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L,
const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
vpColVector &R, vpMatrix <L, vpColVector <R, vpColVector &error_prev, vpColVector &v, double &mu,
vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) {
m_error = R;
if(computeCovariance){
L_true = L;
if(!isoJoIdentity){
vpVelocityTwistMatrix cVo;
cVo.buildFrom(cMo);
LVJ_true = (L*cVo*oJo);
}
}
normRes_1 = normRes;
normRes = 0;
for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
w_true[i] = w[i];
R[i] = R[i] * w[i];
normRes += R[i];
}
if((iter == 0) || compute_interaction){
for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
for(unsigned int j=0; j<6; j++){
L[i][j] *= w[i];
}
}
}
if(isoJoIdentity){
LTL = L.AtA();
computeJTR(L, R, LTR);
switch(m_optimizationMethod){
case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
{
vpMatrix LMA(LTL.getRows(), LTL.getCols());
LMA.eye();
vpMatrix LTLmuI = LTL + (LMA*mu);
v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
if(iter != 0)
mu /= 10.0;
error_prev = m_error;
break;
}
case vpMbTracker::GAUSS_NEWTON_OPT:
default:
v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
}
}
else{
vpVelocityTwistMatrix cVo;
cVo.buildFrom(cMo);
vpMatrix LVJ = (L*cVo*oJo);
vpMatrix LVJTLVJ = (LVJ).AtA();
vpColVector LVJTR;
computeJTR(LVJ, R, LVJTR);
switch(m_optimizationMethod){
case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
{
vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
LMA.eye();
vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
v = cVo * v;
if(iter != 0)
mu /= 10.0;
error_prev = m_error;
break;
}
case vpMbTracker::GAUSS_NEWTON_OPT:
default:
{
v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
v = cVo * v;
break;
}
}
}
cMoPrev = cMo;
ctTc0_Prev = ctTc0;
ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
cMo = ctTc0 * c0Mo;
}