included priors and robust model in example
							parent
							
								
									0fad251355
								
							
						
					
					
						commit
						5a6d719690
					
				|  | @ -85,12 +85,17 @@ int main(const int argc, const char *argv[]){ | |||
| 
 | ||||
|       noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); | ||||
| 
 | ||||
|       NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model)); | ||||
|       NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, | ||||
|           noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), model))); | ||||
|       graph.add(factor); | ||||
|     } | ||||
|     is.ignore(LINESIZE, '\n'); | ||||
|   } | ||||
|   // graph.print("graph");
 | ||||
| 
 | ||||
|   // otherwise GTSAM cannot solve the problem
 | ||||
|   NonlinearFactorGraph graphWithPrior = graph; | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001)); | ||||
|   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); | ||||
| 
 | ||||
|   // GaussNewtonParams parameters;
 | ||||
|   // Stop iterating once the change in error between steps is less than this value
 | ||||
|  | @ -99,7 +104,7 @@ int main(const int argc, const char *argv[]){ | |||
|   // parameters.maxIterations = 100;
 | ||||
|   // Create the optimizer ...
 | ||||
|   std::cout << "Optimizing the factor graph" << std::endl; | ||||
|   GaussNewtonOptimizer optimizer(graph, initial); // , parameters);
 | ||||
|   GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
 | ||||
|   // ... and optimize
 | ||||
|   Values result = optimizer.optimize(); | ||||
|   // result.print("results");
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue