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

fix checks

release/4.3a0
Fan Jiang 2020-06-02 16:18:18 -04:00
parent 15fda68c1a
commit b669e3aa3f
1 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -146,6 +146,14 @@ public:
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
VectorValues d; VectorValues d;
hessianDiagonalAdd(d);
return d;
}
/// Return the diagonal of the Hessian for this factor
virtual void hessianDiagonalAdd(VectorValues &d) const override {
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
for (size_t k = 0; k < size(); ++k) { // for each camera for (size_t k = 0; k < size(); ++k) { // for each camera
Key j = keys_[k]; Key j = keys_[k];
@ -153,7 +161,7 @@ public:
// D x 3 = (D x ZDim) * (ZDim x 3) // D x 3 = (D x ZDim) * (ZDim x 3)
const MatrixZD& Fj = FBlocks_[k]; const MatrixZD& Fj = FBlocks_[k];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose() Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<ZDim, 3>(ZDim * k, 0); * E_.block<ZDim, 3>(ZDim * k, 0);
Eigen::Matrix<double, D, 1> dj; Eigen::Matrix<double, D, 1> dj;
for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@ -163,9 +171,14 @@ public:
// (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
} }
d.insert(j, dj);
auto item = d.find(j);
if(item != d.end()) {
item->second += dj;
} else {
d.emplace(j, dj);
}
} }
return d;
} }
/** /**