Added symmetrization trick to avoid problems with manifold optimization.
parent
b1215a1678
commit
41b840485e
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue