From 361101fdd9bb96c6ff225a7f489287d27e164e83 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:31:05 -0800 Subject: [PATCH 1/5] Improved/refactored example --- python/gtsam_examples/VisualISAM2Example.py | 72 +++++++++++---------- 1 file changed, 37 insertions(+), 35 deletions(-) diff --git a/python/gtsam_examples/VisualISAM2Example.py b/python/gtsam_examples/VisualISAM2Example.py index 4d7889e9b..9dafa13e7 100644 --- a/python/gtsam_examples/VisualISAM2Example.py +++ b/python/gtsam_examples/VisualISAM2Example.py @@ -3,19 +3,23 @@ from __future__ import print_function import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import numpy as np -import time # for sleep() +import time # for sleep() import gtsam from gtsam_examples import SFMdata import gtsam_utils +# shorthand symbols: +X = lambda i: int(gtsam.Symbol('x', i)) +L = lambda j: int(gtsam.Symbol('l', j)) + def visual_ISAM2_plot(poses, points, result): # VisualISAMPlot plots current state of ISAM2 object # Author: Ellon Paiva # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert # Declare an id for the figure - fignum = 0; + fignum = 0 fig = plt.figure(fignum) ax = fig.gca(projection='3d') @@ -23,33 +27,31 @@ def visual_ISAM2_plot(poses, points, result): # Plot points # Can't use data because current frame might not see all points - # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow - # gtsam.plot3DPoints(result, [], marginals); - gtsam_utils.plot3DPoints(fignum, result, 'rx'); + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow + # gtsam.plot3DPoints(result, [], marginals) + gtsam_utils.plot3DPoints(fignum, result, 'rx') # Plot cameras - M = 0; - while result.exists(int(gtsam.Symbol('x',M))): - ii = int(gtsam.Symbol('x',M)); - pose_i = result.pose3_at(ii); - gtsam_utils.plotPose3(fignum, pose_i, 10); - - M = M + 1; + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + gtsam_utils.plotPose3(fignum, pose_i, 10) + i += 1 # draw ax.set_xlim3d(-40, 40) ax.set_ylim3d(-40, 40) ax.set_zlim3d(-40, 40) - plt.ion() - plt.show() - plt.draw() + plt.pause(1) def visual_ISAM2_example(): + plt.ion() + # Define the camera calibration parameters K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model - measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Create the set of ground-truth landmarks points = SFMdata.createPoints() @@ -78,29 +80,29 @@ def visual_ISAM2_example(): for j, point in enumerate(points): camera = gtsam.PinholeCameraCal3_S2(pose, K) measurement = camera.project(point) - graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(gtsam.Symbol('x', i)), int(gtsam.Symbol('l', j)), K)) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K)) # Add an initial guess for the current pose # Intentionally initialize the variables off from the ground truth - initialEstimate.insert(int(gtsam.Symbol('x', i)), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) # If this is the first iteration, add a prior on the first pose to set the coordinate frame # and a prior on the first landmark to set the scale # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before # adding it to iSAM. - if( i == 0): + if(i == 0): # Add a prior on pose x0 - poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(gtsam.PriorFactorPose3(int(gtsam.Symbol('x', 0)), poses[0], poseNoise)) + poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise)) # Add a prior on landmark l0 pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - graph.push_back(gtsam.PriorFactorPoint3(int(gtsam.Symbol('l', 0)), points[0], pointNoise)) # add directly to graph + graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise)) # add directly to graph # Add initial guesses to all observed landmarks # Intentionally initialize the variables off from the ground truth for j, point in enumerate(points): - initialEstimate.insert(int(gtsam.Symbol('l', j)), point + gtsam.Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15)) else: # Update iSAM with the new factors isam.update(graph, initialEstimate) @@ -108,23 +110,23 @@ def visual_ISAM2_example(): # If accuracy is desired at the expense of time, update(*) can be called additional times # to perform multiple optimizer iterations every step. isam.update() - currentEstimate = isam.calculate_estimate(); - print( "****************************************************" ) - print( "Frame", i, ":" ) - for j in range(i+1): - print( gtsam.Symbol('x',j) ) - print( currentEstimate.pose3_at(int(gtsam.Symbol('x',j))) ) + currentEstimate = isam.calculate_estimate() + print("****************************************************") + print("Frame", i, ":") + for j in range(i + 1): + print(X(j), ":", currentEstimate.atPose3(X(j))) for j in range(len(points)): - print( gtsam.Symbol('l',j) ) - print( currentEstimate.point3_at(int(gtsam.Symbol('l',j))) ) + print(L(j), ":", currentEstimate.atPoint3(L(j))) - visual_ISAM2_plot(poses, points, currentEstimate); - time.sleep(1) + visual_ISAM2_plot(poses, points, currentEstimate) # Clear the factor graph and values for the next iteration - graph.resize(0); - initialEstimate.clear(); + graph.resize(0) + initialEstimate.clear() + + plt.ioff() + plt.show() if __name__ == '__main__': visual_ISAM2_example() From 1b9b90803abc4807765c63ffab1a7dfb0713e12f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:31:44 -0800 Subject: [PATCH 2/5] Committed to MATLAB atT methods --- python/handwritten/nonlinear/Values.cpp | 76 ++++++------------------- 1 file changed, 16 insertions(+), 60 deletions(-) diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 6715fb071..84e82f376 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -26,61 +26,17 @@ #include "gtsam/geometry/Point3.h" #include "gtsam/geometry/Rot3.h" #include "gtsam/geometry/Pose3.h" +#include "gtsam/navigation/ImuBias.h" using namespace boost::python; using namespace gtsam; -/** The function ValuesAt is a workaround to be able to call the correct templated version - * of Values::at. Without it, python would only try to match the last 'at' metho defined - * below. With this wrapper function we can call 'at' in python passing an extra type, - * which will define the type to be returned. Example: - * - * >>> import gtsam - * >>> v = gtsam.nonlinear.Values() - * >>> v.insert(1,gtsam.geometry.Point3()) - * >>> v.insert(2,gtsam.geometry.Rot3()) - * >>> v.insert(3,gtsam.geometry.Pose3()) - * >>> v.at(1,gtsam.geometry.Point3()) - * >>> v.at(2,gtsam.geometry.Rot3()) - * >>> v.at(3,gtsam.geometry.Pose3()) - * - * A more 'pythonic' way I think would be to not use this function and define different - * 'at' methods below using the name of the type in the function name, like: - * - * .def("point3_at", &Values::at, return_internal_reference<>()) - * .def("rot3_at", &Values::at, return_internal_reference<>()) - * .def("pose3_at", &Values::at, return_internal_reference<>()) - * - * and then they could be accessed from python as - * - * >>> import gtsam - * >>> v = gtsam.nonlinear.Values() - * >>> v.insert(1,gtsam.geometry.Point3()) - * >>> v.insert(2,gtsam.geometry.Rot3()) - * >>> v.insert(3,gtsam.geometry.Pose3()) - * >>> v.point3_at(1) - * >>> v.rot3_at(2) - * >>> v.pose3_at(3) - * - * In fact, I just saw the pythonic way sounds more clear, so I'm sticking with this and - * leaving the comments here for future reference. I'm using the PEP0008 for method naming. - * See: https://www.python.org/dev/peps/pep-0008/#function-and-method-arguments - */ -// template -// const T & ValuesAt( const Values & v, Key j, T /*type*/) -// { -// 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 - // will compile, but are useless in the python wrapper. We need to use specific - // 'at' and 'insert' methods for each type. - // const Value& (Values::*at1)(Key) const = &Values::at; - // void (Values::*insert1)(Key, const Value&) = &Values::insert; + typedef imuBias::ConstantBias Bias; + 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; @@ -88,7 +44,8 @@ void exportValues(){ 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; - + void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; + void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; class_("Values", init<>()) .def(init()) @@ -101,23 +58,22 @@ void exportValues(){ .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) - // NOTE: The following commented lines are another way of specializing the return type. - // See long comment above. - // .def("at", &ValuesAt, return_internal_reference<>()) - // .def("at", &ValuesAt, return_internal_reference<>()) - // .def("at", &ValuesAt, return_internal_reference<>()) - .def("point3_at", &Values::at, return_value_policy()) - .def("rot3_at", &Values::at, return_value_policy()) - .def("pose3_at", &Values::at, return_value_policy()) + .def("insert", insert_bias) + .def("insert", insert_vector3) + .def("atPoint2", &Values::at, return_value_policy()) + .def("atRot2", &Values::at, return_value_policy()) + .def("atPose2", &Values::at, return_value_policy()) + .def("atPoint3", &Values::at, return_value_policy()) + .def("atRot3", &Values::at, return_value_policy()) + .def("atPose3", &Values::at, return_value_policy()) + .def("atConstantBias", &Values::at, return_value_policy()) + .def("atVector3", &Values::at, return_value_policy()) .def("exists", exists1) .def("keys", &Values::keys) ; From 659caa58c1d838bf092052b379ee336991169984 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:32:02 -0800 Subject: [PATCH 3/5] getNonlinearFactor --- .../nonlinear/NonlinearFactorGraph.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp index f1e14deda..4ba4ba008 100644 --- a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @brief exports NonlinearFactorGraph class to python - * @author Andrew Melim + * @author Andrew Melim * @author Ellon Paiva Mendes (LAAS-CNRS) **/ @@ -28,6 +28,13 @@ using namespace gtsam; BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1); +boost::shared_ptr getNonlinearFactor( + const NonlinearFactorGraph& graph, size_t idx) { + auto p = boost::dynamic_pointer_cast(graph.at(idx)); + if (!p) throw std::runtime_error("No NonlinearFactor at requested index"); + return p; +}; + void exportNonlinearFactorGraph(){ typedef NonlinearFactorGraph::sharedFactor sharedFactor; @@ -36,7 +43,7 @@ void exportNonlinearFactorGraph(){ void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; class_("NonlinearFactorGraph", init<>()) - .def("size",&NonlinearFactorGraph::size) + .def("size",&NonlinearFactorGraph::size) .def("push_back", push_back1) .def("add", add1) .def("resize", &NonlinearFactorGraph::resize) @@ -44,4 +51,6 @@ void exportNonlinearFactorGraph(){ .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) ; + def("getNonlinearFactor", getNonlinearFactor); + } From c4494ba9691a6c132d0d1be9ee72f550e606f626 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:32:20 -0800 Subject: [PATCH 4/5] Small changes --- python/handwritten/slam/BetweenFactor.cpp | 18 +++++++++--------- python/handwritten/slam/PriorFactor.cpp | 7 ++++--- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp index b6fc552a0..0e0949fb5 100644 --- a/python/handwritten/slam/BetweenFactor.cpp +++ b/python/handwritten/slam/BetweenFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @brief wraps BetweenFactor for several values to python - * @author Andrew Melim + * @author Andrew Melim * @author Ellon Paiva Mendes (LAAS-CNRS) **/ @@ -33,17 +33,17 @@ using namespace gtsam; using namespace std; -// template +// template // void exportBetweenFactor(const std::string& name){ -// class_(name, init<>()) -// .def(init()) +// class_(name, init<>()) +// .def(init()) // ; // } -#define BETWEENFACTOR(VALUE) \ - class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#VALUE) \ - .def(init()) \ - .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ +#define BETWEENFACTOR(T) \ + class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#T) \ + .def(init()) \ + .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ ; void exportBetweenFactors() diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp index dcb9de8ea..3ada91f43 100644 --- a/python/handwritten/slam/PriorFactor.cpp +++ b/python/handwritten/slam/PriorFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @brief wraps PriorFactor for several values to python - * @author Andrew Melim + * @author Andrew Melim * @author Ellon Paiva Mendes (LAAS-CNRS) **/ @@ -54,4 +54,5 @@ void exportPriorFactors() PRIORFACTOR(Point3) PRIORFACTOR(Rot3) PRIORFACTOR(Pose3) -} \ No newline at end of file + PRIORFACTOR(Vector3) +} From 1d62faa5a5a055e32ea34a96ffad0c2056fdf4c1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 01:32:36 -0800 Subject: [PATCH 5/5] Refactored plot without underscores --- python/gtsam_utils/__init__.py | 2 +- python/gtsam_utils/plot.py | 59 ++++++++++++++++++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) create mode 100644 python/gtsam_utils/plot.py diff --git a/python/gtsam_utils/__init__.py b/python/gtsam_utils/__init__.py index 0ef2dfcd3..56c55aa94 100644 --- a/python/gtsam_utils/__init__.py +++ b/python/gtsam_utils/__init__.py @@ -1 +1 @@ -from ._plot import * +from .plot import * diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py new file mode 100644 index 000000000..84a388bbb --- /dev/null +++ b/python/gtsam_utils/plot.py @@ -0,0 +1,59 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3(fignum, point, linespec): + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + ax.plot([point.x()], [point.y()], [point.z()], linespec) + + +def plot3DPoints(fignum, values, linespec, marginals=None): + # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances + # Finds all the Point3 objects in the given Values object and plots them. + # If a Marginals object is given, this function will also plot marginal + # covariance ellipses for each point. + + keys = values.keys() + + # Plot points and covariance matrices + for key in keys: + try: + p = values.atPoint3(key); + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plotPoint3(p, linespec, P); + # else + plotPoint3(fignum, p, linespec); + except RuntimeError: + continue + # I guess it's not a Point3 + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + C = pose.translation().vector() + + # draw the camera axes + xAxis = C + gRp[:, 0] * axisLength + L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-') + + yAxis = C + gRp[:, 1] * axisLength + L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-') + + zAxis = C + gRp[:, 2] * axisLength + L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-') + + # # plot the covariance + # if (nargin>2) && (~isempty(P)) + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + # gtsam.covarianceEllipse3D(C,gPp); + # end