12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

optimized point damping

release/4.3a0
Luca 2014-04-23 15:15:48 -04:00
parent e56666e85c
commit 2b52bad569
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/slam/SmartFactorBase.h
View File

@ -302,15 +302,18 @@ public:
// Point covariance inv(E'*E)
Matrix3 EtE = E.transpose() * E;
Matrix3 DMatrix = eye(E.cols()); // damping matrix
if (diagonalDamping) { // diagonal of the hessian
DMatrix(0, 0) = EtE(0, 0);
DMatrix(1, 1) = EtE(1, 1);
DMatrix(2, 2) = EtE(2, 2);
EtE(0, 0) += lambda * EtE(0, 0);
EtE(1, 1) += lambda * EtE(1, 1);
EtE(2, 2) += lambda * EtE(2, 2);
}else{
EtE(0, 0) += lambda;
EtE(1, 1) += lambda;
EtE(2, 2) += lambda;
}
PointCov.noalias() = (EtE + lambda * DMatrix).inverse();
PointCov.noalias() = (EtE).inverse();
return f;
}