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