Added symmetrization trick to avoid problems with manifold optimization.

gtsam_unstable/linear/QPSolver.h

@@ -37,8 +37,13 @@ struct QPPolicy {
     GaussianFactorGraph no_constant_factor;
     for (auto factor : qp.cost) {
       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
         // 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);
     }