diff --git a/python/gtsam_examples/OdometryExample.py b/python/gtsam_examples/OdometryExample.py index 45f729a89..0800bab4e 100644 --- a/python/gtsam_examples/OdometryExample.py +++ b/python/gtsam_examples/OdometryExample.py @@ -1,7 +1,36 @@ #!/usr/bin/env python -from gtsam import * -import numpy_eigen as npe +from __future__ import print_function +import gtsam +import numpy as np -noisemodel = DiagonalNoiseModel.Sigmas([0.1, 0.1, 0.1]) -graph = NonlinearFactorGraph() -initialEstimate = Values() \ No newline at end of file +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + +# Add odometry factors +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# For simplicity, we will use the same noise model for each odometry factor +odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) +graph.print("\nFactor Graph:\n") + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) +initial.print("\nInitial Estimate:\n") + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +result.print("\nFinal Result:\n") diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp index 44b11f00b..7c7cdf3cc 100644 --- a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -9,8 +9,19 @@ using namespace boost::python; using namespace gtsam; void exportLevenbergMarquardtOptimizer(){ + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) + ; + class_("LevenbergMarquardtOptimizer", init()) .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) ; -} \ No newline at end of file +} diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp index 4200a150e..f1e14deda 100644 --- a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -26,6 +26,7 @@ using namespace boost::python; using namespace gtsam; +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1); void exportNonlinearFactorGraph(){ @@ -40,6 +41,7 @@ void exportNonlinearFactorGraph(){ .def("add", add1) .def("resize", &NonlinearFactorGraph::resize) .def("empty", &NonlinearFactorGraph::empty) + .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) ; -} \ No newline at end of file +} diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 0302abbe2..6715fb071 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -20,6 +20,9 @@ #include #include "gtsam/nonlinear/Values.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" #include "gtsam/geometry/Point3.h" #include "gtsam/geometry/Rot3.h" #include "gtsam/geometry/Pose3.h" @@ -69,6 +72,8 @@ using namespace gtsam; // return v.at(j); // } +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1); + void exportValues(){ // NOTE: Apparently the class 'Value'' is deprecated, so the commented lines below @@ -77,10 +82,14 @@ void exportValues(){ // const Value& (Values::*at1)(Key) const = &Values::at; // void (Values::*insert1)(Key, const Value&) = &Values::insert; bool (Values::*exists1)(Key) const = &Values::exists; + void (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert; + void (Values::*insert_rot2) (Key, const gtsam::Rot2&) = &Values::insert; + void (Values::*insert_pose2) (Key, const gtsam::Pose2&) = &Values::insert; void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert; void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; + class_("Values", init<>()) .def(init()) .def("clear", &Values::clear) @@ -89,12 +98,15 @@ void exportValues(){ .def("equals", &Values::equals) .def("erase", &Values::erase) .def("insert_fixed", &Values::insertFixed) - .def("print", &Values::print) + .def("print", &Values::print, print_overloads(args("s"))) .def("size", &Values::size) .def("swap", &Values::swap) // NOTE: Following commented lines add useless methods on Values // .def("insert", insert1) // .def("at", at1, return_value_policy()) + .def("insert", insert_point2) + .def("insert", insert_rot2) + .def("insert", insert_pose2) .def("insert", insert_point3) .def("insert", insert_rot3) .def("insert", insert_pose3) @@ -109,4 +121,4 @@ void exportValues(){ .def("exists", exists1) .def("keys", &Values::keys) ; -} \ No newline at end of file +}