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

Added symmetrization trick to avoid problems with manifold optimization.

release/4.3a0
= 2016-09-04 15:34:33 -04:00
parent b1215a1678
commit 41b840485e
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam_unstable/linear/QPSolver.h
View File

@ -37,8 +37,13 @@ struct QPPolicy {
GaussianFactorGraph no_constant_factor; GaussianFactorGraph no_constant_factor;
for (auto factor : qp.cost) { for (auto factor : qp.cost) {
HessianFactor hf = static_cast<HessianFactor>(*factor); HessianFactor hf = static_cast<HessianFactor>(*factor);
//a trick to ensure that the augmented matrix is always symmetric. Should only be an issue when dealing
// with the manifold.
hf.augmentedInformation() = (hf.augmentedInformation() + hf.augmentedInformation().transpose())/2;
if (hf.constantTerm() < 0) // Hessian Factors cannot deal if (hf.constantTerm() < 0) // Hessian Factors cannot deal
// with negative constant terms replace with zero in this case // with negative constant terms replace with zero in this case
//TODO: Perhaps there is a smarter way to set the constant term such that the resulting matrix is almost always
// Positive definite.
hf.constantTerm() = 0.0; hf.constantTerm() = 0.0;
no_constant_factor.push_back(hf); no_constant_factor.push_back(hf);
} }