diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 6f1b35386..e283c7911 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -94,6 +94,13 @@ namespace pose2SLAM { /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, const SharedNoiseModel& model); + + /// Optimize + Values optimize(const Values& initialEstimate) { + typedef NonlinearOptimizer Optimizer; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); + } }; /// The sequential optimizer