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

Address comments

release/4.3a0
Fan Jiang 2020-06-02 23:06:00 -04:00
parent 9186e656a5
commit e8111a129d
3 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/linear/HessianFactor.cpp
View File

@ -315,6 +315,7 @@ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
auto result = d.emplace(keys_[j], info_.diagonal(j)); auto result = d.emplace(keys_[j], info_.diagonal(j));
if(!result.second) { if(!result.second) {
// if emplace fails, it returns an iterator to the existing element, which we add to:
result.first->second += info_.diagonal(j); result.first->second += info_.diagonal(j);
} }
} }

6
gtsam/linear/linearAlgorithms-inst.h
View File

@ -100,9 +100,9 @@ namespace gtsam
for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
if(!result.second) if(!result.second)
throw std::invalid_argument( throw std::runtime_error(
"Requested to emplace variable '" + DefaultKeyFormatter(*frontal) "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
+ "' already in this VectorValues."); + "' that exists.");
VectorValues::const_iterator r = result.first; VectorValues::const_iterator r = result.first;
myData.cliqueResults.emplace(r->first, r); myData.cliqueResults.emplace(r->first, r);

2
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -151,7 +151,7 @@ public:
return d; return d;
} }
/// Return the diagonal of the Hessian for this factor /// Add the diagonal of the Hessian for this factor to existing VectorValues
virtual void hessianDiagonalAdd(VectorValues &d) const override { virtual void hessianDiagonalAdd(VectorValues &d) const override {
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // 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