12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

single GN iteration now

release/4.3a0
Luca 2014-05-21 14:28:39 -04:00
parent 8015392610
commit bf8acfda9a
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/nonlinear/LagoInitializer.cpp
View File

@ -232,7 +232,9 @@ Values initializeLago(const NonlinearFactorGraph& graph) {
pose2Graph.add(PriorFactor<Pose2>(keyAnchor, Pose2(), priorPose2Noise)); pose2Graph.add(PriorFactor<Pose2>(keyAnchor, Pose2(), priorPose2Noise));
// Optimize Pose2, with initialGuessLago as initial guess // Optimize Pose2, with initialGuessLago as initial guess
GaussNewtonOptimizer pose2optimizer(pose2Graph, initialGuessLago); GaussNewtonParams params;
params.setMaxIterations(1);
GaussNewtonOptimizer pose2optimizer(pose2Graph, initialGuessLago, params);
initialGuessLago = pose2optimizer.optimize(); initialGuessLago = pose2optimizer.optimize();
initialGuessLago.erase(keyAnchor); // that was fictitious initialGuessLago.erase(keyAnchor); // that was fictitious
return initialGuessLago; return initialGuessLago;