fix checks
parent
15fda68c1a
commit
b669e3aa3f
|
@ -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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in New Issue