From 41b840485e2dadbae0a25b862a56a68bff535bc4 Mon Sep 17 00:00:00 2001 From: = Date: Sun, 4 Sep 2016 15:34:33 -0400 Subject: [PATCH] Added symmetrization trick to avoid problems with manifold optimization. --- gtsam_unstable/linear/QPSolver.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 2c59423fe..d52644975 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -37,8 +37,13 @@ struct QPPolicy { GaussianFactorGraph no_constant_factor; for (auto factor : qp.cost) { HessianFactor hf = static_cast(*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 // 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; no_constant_factor.push_back(hf); }