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

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];
@ -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;
} }
/** /**