From 4ba7c59330cda33ec496ebcf1bdf6e7f97e13ffe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 27 Sep 2018 22:45:14 -0400 Subject: [PATCH 01/14] Plotting utilities --- cython/gtsam/utils/plot.py | 76 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 cython/gtsam/utils/plot.py diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py new file mode 100644 index 000000000..556e9d8d5 --- /dev/null +++ b/cython/gtsam/utils/plot.py @@ -0,0 +1,76 @@ +"""Various plotting utlities.""" + +import numpy as np +import matplotlib.pyplot as plt + + +def plot_point3_on_axes(axes, point, linespec): + """Plot a 3D point on given axis 'axes' with given 'linespec'.""" + axes.plot([point.x()], [point.y()], [point.z()], linespec) + + +def plot_point3(fignum, point, linespec): + """Plot a 3D point on given figure with given 'linespec'.""" + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plot_point3_on_axes(axes, point, linespec) + + +def plot_3d_points(fignum, values, linespec, marginals=None): + """ + Plots the Point3s in '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 i in range(keys.size()): + try: + p = values.atPoint3(keys.at(i)) + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plot_point3(p, linespec, P); + # else + plot_point3(fignum, p, linespec) + except RuntimeError: + continue + # I guess it's not a Point3 + + +def plot_pose3_on_axes(axes, pose, axis_length=0.1): + """Plot a 3D pose on given axis 'axes' with given 'axis_length'.""" + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + t = pose.translation() + origin = np.array([t.x(), t.y(), t.z()]) + + # draw the camera axes + x_axis = origin + gRp[:, 0] * axis_length + line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-') + + y_axis = origin + gRp[:, 1] * axis_length + line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-') + + z_axis = origin + gRp[:, 2] * axis_length + line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], line[:, 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(origin,gPp); + # end + + +def plot_pose3(fignum, pose, axis_length=0.1): + """Plot a 3D pose on given figure with given 'axis_length'.""" + # get figure object + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plot_pose3_on_axes(axes, pose, axis_length) From 7097880d490f67166e16db5f5a655577a67b3b95 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 27 Sep 2018 22:46:24 -0400 Subject: [PATCH 02/14] GTSAM examples, cython versions --- cython/gtsam/examples/OdometryExample.py | 38 +++++ cython/gtsam/examples/Pose2SLAMExample.py | 75 ++++++++++ cython/gtsam/examples/README.md | 4 + cython/gtsam/examples/SFMdata.py | 37 +++++ cython/gtsam/examples/VisualISAM2Example.py | 153 ++++++++++++++++++++ cython/gtsam/examples/__init__.py | 1 + 6 files changed, 308 insertions(+) create mode 100644 cython/gtsam/examples/OdometryExample.py create mode 100644 cython/gtsam/examples/Pose2SLAMExample.py create mode 100644 cython/gtsam/examples/README.md create mode 100644 cython/gtsam/examples/SFMdata.py create mode 100644 cython/gtsam/examples/VisualISAM2Example.py create mode 100644 cython/gtsam/examples/__init__.py diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py new file mode 100644 index 000000000..3bdf6eda5 --- /dev/null +++ b/cython/gtsam/examples/OdometryExample.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python +from __future__ import print_function + +import numpy as np + +import gtsam + +# 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/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py new file mode 100644 index 000000000..fe71788d8 --- /dev/null +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -0,0 +1,75 @@ +from __future__ import print_function + +import math + +import numpy as np + +import gtsam + + +def Vector3(x, y, z): return np.array([x, y, z]) + + +# 1. Create a factor graph container and add factors to it +graph = gtsam.NonlinearFactorGraph() + +# 2a. 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) +priorNoise = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) +graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise)) + +# For simplicity, we will use the same noise model for odometry and loop closures +model = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) + +# 2b. Add odometry factors +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model)) +graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.Pose2(2, 0, math.pi / 2), model)) + +# 2c. Add the loop closure constraint +# This factor encodes the fact that we have returned to the same pose. In real +# systems, these constraints may be identified in many ways, such as appearance-based +# techniques with camera images. We will use another Between Factor to enforce this constraint: +graph.add(gtsam.BetweenFactorPose2( + 5, 2, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.print_("\nFactor Graph:\n") # print + +# 3. Create the data structure to hold the initial_estimate estimate to the +# solution. For illustrative purposes, these have been deliberately set to incorrect values +initial_estimate = gtsam.Values() +initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) +initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) +initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) +initial_estimate.print_("\nInitial Estimate:\n") # print + +# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer +# The optimizer accepts an optional set of configuration parameters, +# controlling things like convergence criteria, the type of linear +# system solver to use, and the amount of information displayed during +# optimization. We will set a few parameters as a demonstration. +parameters = gtsam.GaussNewtonParams() + +# Stop iterating once the change in error between steps is less than this value +parameters.setRelativeErrorTol(1e-5) +# Do not perform more than N iteration steps +parameters.setMaxIterations(100) +# Create the optimizer ... +optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) +# ... and optimize +result = optimizer.optimize() +result.print_("Final Result:\n") + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +print("x1 covariance:\n", marginals.marginalCovariance(1)) +print("x2 covariance:\n", marginals.marginalCovariance(2)) +print("x3 covariance:\n", marginals.marginalCovariance(3)) +print("x4 covariance:\n", marginals.marginalCovariance(4)) +print("x5 covariance:\n", marginals.marginalCovariance(5)) diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md new file mode 100644 index 000000000..4474da488 --- /dev/null +++ b/cython/gtsam/examples/README.md @@ -0,0 +1,4 @@ +These examples are almost identical to the old handwritten python wrapper +examples. However, there are just some slight name changes, for example .print +becomes .print_, and noiseModel.Diagonal becomes noiseModel_Diagonal etc... +Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0)) diff --git a/cython/gtsam/examples/SFMdata.py b/cython/gtsam/examples/SFMdata.py new file mode 100644 index 000000000..742518c2c --- /dev/null +++ b/cython/gtsam/examples/SFMdata.py @@ -0,0 +1,37 @@ +""" +A structure-from-motion example with landmarks + - The landmarks form a 10 meter cube + - The robot rotates around the landmarks, always facing towards the cube +""" + +import numpy as np + +import gtsam + + +def createPoints(): + # Create the set of ground-truth landmarks + points = [gtsam.Point3(10.0, 10.0, 10.0), + gtsam.Point3(-10.0, 10.0, 10.0), + gtsam.Point3(-10.0, -10.0, 10.0), + gtsam.Point3(10.0, -10.0, 10.0), + gtsam.Point3(10.0, 10.0, -10.0), + gtsam.Point3(-10.0, 10.0, -10.0), + gtsam.Point3(-10.0, -10.0, -10.0), + gtsam.Point3(10.0, -10.0, -10.0)] + return points + + +def createPoses(K): + # Create the set of ground-truth poses + radius = 30.0 + angles = np.linspace(0, 2*np.pi, 8, endpoint=False) + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + poses = [] + for theta in angles: + position = gtsam.Point3(radius*np.cos(theta), + radius*np.sin(theta), 0.0) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) + poses.append(camera.pose()) + return poses diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py new file mode 100644 index 000000000..29383612d --- /dev/null +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -0,0 +1,153 @@ +"""An example of running visual SLAM using iSAM2.""" +# pylint: disable=invalid-name + +from __future__ import print_function + +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 + +import gtsam +import gtsam.utils.plot as gtsam_plot +from gtsam.examples import SFMdata + + +def X(i): + """Create key for pose i.""" + return int(gtsam.symbol(ord('x'), i)) + + +def L(j): + """Create key for landmark j.""" + return int(gtsam.symbol(ord('l'), j)) + + +def visual_ISAM2_plot(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 + + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plt.cla() + + # Plot points + # Can't use data because current frame might not see all points + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) + # gtsam.plot_3d_points(result, [], marginals) + gtsam_plot.plot_3d_points(fignum, result, 'rx') + + # Plot cameras + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + gtsam_plot.plot_pose3(fignum, pose_i, 10) + i += 1 + + # draw + axes.set_xlim3d(-40, 40) + axes.set_ylim3d(-40, 40) + axes.set_zlim3d(-40, 40) + 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 + measurement_noise = gtsam.noiseModel_Isotropic.Sigma( + 2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses(K) + + # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps + # to maintain proper linearization and efficient variable ordering, iSAM2 + # performs partial relinearization/reordering at each step. A parameter + # structure is available that allows the user to set various properties, such + # as the relinearization threshold and type of linear solver. For this + # example, we we set the relinearization threshold small so the iSAM2 result + # will approach the batch result. + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.01) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + # Create a Factor Graph and Values to hold the new data + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + + # Add factors for each landmark observation + for j, point in enumerate(points): + camera = gtsam.PinholeCameraCal3_S2(pose, K) + measurement = camera.project(point) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2( + measurement, measurement_noise, X(i), L(j), K)) + + # Add an initial guess for the current pose + # Intentionally initialize the variables off from the ground truth + initial_estimate.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: + # Add a prior on pose x0 + pose_noise = 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], pose_noise)) + + # Add a prior on landmark l0 + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + graph.push_back(gtsam.PriorFactorPoint3( + L(0), points[0], point_noise)) # 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): + initial_estimate.insert(L(j), gtsam.Point3( + point.x()-0.25, point.y()+0.20, point.z()+0.15)) + else: + # Update iSAM with the new factors + isam.update(graph, initial_estimate) + # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + # If accuracy is desired at the expense of time, update(*) can be called additional + # times to perform multiple optimizer iterations every step. + isam.update() + current_estimate = isam.calculateEstimate() + print("****************************************************") + print("Frame", i, ":") + for j in range(i + 1): + print(X(j), ":", current_estimate.atPose3(X(j))) + + for j in range(len(points)): + print(L(j), ":", current_estimate.atPoint3(L(j))) + + visual_ISAM2_plot(current_estimate) + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initial_estimate.clear() + + plt.ioff() + plt.show() + + +if __name__ == '__main__': + visual_ISAM2_example() diff --git a/cython/gtsam/examples/__init__.py b/cython/gtsam/examples/__init__.py new file mode 100644 index 000000000..ccdd2d6d2 --- /dev/null +++ b/cython/gtsam/examples/__init__.py @@ -0,0 +1 @@ +from . import SFMdata From 3de391e895e651019d7f8c5f74b3c2afe1fd12ff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 27 Sep 2018 22:47:15 -0400 Subject: [PATCH 03/14] Ignore .env file for VS code --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index b62617d21..e9ac11c72 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,4 @@ cython/gtsam.pyx cython/gtsam.so cython/gtsam_wrapper.pxd .vscode +/.env From 650b65f1fb69d9eacb090d597b1bb02f7735f7cb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 18:43:06 -0400 Subject: [PATCH 04/14] Kevin's port from cpp --- cython/gtsam/examples/PlanarSLAMExample.py | 109 +++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 cython/gtsam/examples/PlanarSLAMExample.py diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py new file mode 100644 index 000000000..ca3d26bee --- /dev/null +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -0,0 +1,109 @@ +import gtsam +import numpy as np +import math + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Create the keys we need for graph +# static Symbol x1('x',1), x2('x',2), x3('x',3); +# static Symbol l1('l',1), l2('l',2); +x1 = gtsam.symbol(ord('x'), 1) +x2 = gtsam.symbol(ord('x'), 2) +x3 = gtsam.symbol(ord('x'), 3) +l1 = gtsam.symbol(ord('l'), 4) +l2 = gtsam.symbol(ord('l'), 5) + +# Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) +# Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +# noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta +priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +# graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph +graph.add(gtsam.PriorFactorPose2(x1, priorMean, priorNoise)) + + +# Add two odometry factors between x1,x2 and x2,x3 +# Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta +# 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.emplace_shared >(x1, x2, odometry, odometryNoise); +graph.add(gtsam.BetweenFactorPose2(x1, x2, odometry, odometryNoise)) +# graph.emplace_shared >(x2, x3, odometry, odometryNoise); +graph.add(gtsam.BetweenFactorPose2(x2, x3, odometry, odometryNoise)) + +# Add Range-Bearing measurements to two different landmarks +# create a noise model for the landmark measurements +# noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range +measurementNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) +# Rot2 bearing11 = Rot2::fromDegrees(45), +# bearing21 = Rot2::fromDegrees(90), +# bearing32 = Rot2::fromDegrees(90); +# double range11 = std::sqrt(4.0+4.0), +# range21 = 2.0, +# range32 = 2.0; +bearing11 = gtsam.Rot2.fromDegrees(np.pi/4) +bearing21 = gtsam.Rot2.fromDegrees(np.pi/2) +bearing32 = gtsam.Rot2.fromDegrees(np.pi/2) +range11 = np.sqrt(4.0+4.0) +range21 = 2.0 +range32 = 2.0 + +# Add Bearing-Range factors +# graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); +# graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); +# graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); +graph.add(gtsam.BearingRangeFactor2D(x1,l1,bearing11,range11,measurementNoise)) +graph.add(gtsam.BearingRangeFactor2D(x2,l1,bearing21,range21,measurementNoise)) +graph.add(gtsam.BearingRangeFactor2D(x3,l2,bearing32,range32,measurementNoise)) + +# Print graph +graph.print_("Factor Graph:\n"); + +# Create (deliberately inaccurate) initial estimate +# Values initialEstimate; +# initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); +# initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); +# initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); +# initialEstimate.insert(l1, Point2(1.8, 2.1)); +# initialEstimate.insert(l2, Point2(4.1, 1.8)); +initialEstimate = gtsam.Values() +initialEstimate.insert(x1, gtsam.Pose2(-0.25, 0.20, 0.15)) +initialEstimate.insert(x2, gtsam.Pose2( 2.30, 0.10, -0.20)) +initialEstimate.insert(x3, gtsam.Pose2( 4.10, 0.10, 0.10)) +initialEstimate.insert(l1, gtsam.Point2( 1.80, 2.10)) +initialEstimate.insert(l2, gtsam.Point2( 4.10, 1.80)) + +# Print +initialEstimate.print_("Initial Estimate:\n"); + +# Optimize using Levenberg-Marquardt optimization. The optimizer +# accepts an optional set of configuration parameters, controlling +# things like convergence criteria, the type of linear system solver +# to use, and the amount of information displayed during optimization. +# Here we will use the default set of parameters. See the +# documentation for the full set of parameters. +# LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); +# Values result = optimizer.optimize(); +# result.print("Final Result:\n"); +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate, params) +result = optimizer.optimize() +result.print_("\nFinal Result:\n") + +# Calculate and print marginal covariances for all variables +# Marginals marginals(graph, result); +# print(marginals.marginalCovariance(x1), "x1 covariance"); +# print(marginals.marginalCovariance(x2), "x2 covariance"); +# print(marginals.marginalCovariance(x3), "x3 covariance"); +# print(marginals.marginalCovariance(l1), "l1 covariance"); +# print(marginals.marginalCovariance(l2), "l2 covariance"); +marginals = gtsam.Marginals(graph, result) +print("x1 covariance:\n", marginals.marginalCovariance(x1)) +print("x2 covariance:\n", marginals.marginalCovariance(x2)) +print("x3 covariance:\n", marginals.marginalCovariance(x3)) +print("x4 covariance:\n", marginals.marginalCovariance(l1)) +print("x5 covariance:\n", marginals.marginalCovariance(l2)) From c2541c0fc8e76a2df34925e737d02bb3ad168119 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Sat, 13 Oct 2018 22:46:20 -0400 Subject: [PATCH 05/14] re-cythonize if pyx file is updated. --- cmake/GtsamCythonWrap.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 6f378f77d..356ed0d69 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -95,7 +95,7 @@ function(cythonize target pyx_file output_lib_we output_dir include_dirs libs in # Late dependency injection, to make sure this gets called whenever the interface header is updated # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes - add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} APPEND) + add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND) if (NOT "${dependencies}" STREQUAL "") add_dependencies(${target}_pyx2cpp "${dependencies}") endif() From e807d62980ef25395c136f74e4072e1fbc40c3c3 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Sat, 13 Oct 2018 23:06:04 -0400 Subject: [PATCH 06/14] upgrade eigency to latest version with bug fixes. --- cython/gtsam_eigency/eigency_cpp.h | 100 ++++++++++++++++++++++------- 1 file changed, 76 insertions(+), 24 deletions(-) diff --git a/cython/gtsam_eigency/eigency_cpp.h b/cython/gtsam_eigency/eigency_cpp.h index 45ac6caaa..923ca7c94 100644 --- a/cython/gtsam_eigency/eigency_cpp.h +++ b/cython/gtsam_eigency/eigency_cpp.h @@ -306,10 +306,17 @@ public: : cols), stride ) {} + + MapBase &operator=(const MatrixType &other) { + Base::operator=(other); + return *this; + } + + virtual ~MapBase() { } }; -template class DenseBase, +template class EigencyDenseBase, typename Scalar, int _Rows, int _Cols, int _Options = Eigen::AutoAlign | @@ -341,16 +348,18 @@ template class DenseBase, int _StrideOuter=0, int _StrideInner=0, int _MaxRows = _Rows, int _MaxCols = _Cols> -class FlattenedMap: public MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > { +class FlattenedMap: public MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > { public: - typedef MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base; + typedef MapBase, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base; FlattenedMap() - : Base(NULL, 0, 0) {} + : Base(NULL, 0, 0), + object_(NULL) {} FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0) : Base(data, rows, cols, - Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)) { + Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)), + object_(NULL) { } FlattenedMap(PyArrayObject *object) @@ -359,18 +368,26 @@ public: (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0], Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, - _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])) { + _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])), + object_(object) { if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object)) throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); + + Py_XINCREF(object_); } FlattenedMap &operator=(const FlattenedMap &other) { - // Replace the memory that we point to (not a memory allocation) - new (this) FlattenedMap(const_cast(other.data()), - other.rows(), - other.cols(), - other.outerStride(), - other.innerStride()); + if (other.object_) { + new (this) FlattenedMap(other.object_); + } else { + // Replace the memory that we point to (not a memory allocation) + new (this) FlattenedMap(const_cast(other.data()), + other.rows(), + other.cols(), + other.outerStride(), + other.innerStride()); + } + return *this; } @@ -382,9 +399,16 @@ public: return static_cast(*this); } - operator DenseBase() const { - return DenseBase(static_cast(*this)); + operator EigencyDenseBase() const { + return EigencyDenseBase(static_cast(*this)); } + + virtual ~FlattenedMap() { + Py_XDECREF(object_); + } + +private: + PyArrayObject * const object_; }; @@ -394,39 +418,60 @@ public: typedef MapBase Base; typedef typename MatrixType::Scalar Scalar; + enum { + RowsAtCompileTime = Base::Base::RowsAtCompileTime, + ColsAtCompileTime = Base::Base::ColsAtCompileTime + }; + Map() - : Base(NULL, 0, 0) { + : Base(NULL, + (RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime, + (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime), + object_(NULL) { } Map(Scalar *data, long rows, long cols) - : Base(data, rows, cols) {} + : Base(data, rows, cols), + object_(NULL) {} Map(PyArrayObject *object) : Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data, // ROW: If array is in row-major order, transpose (see README) (PyObject*)object == Py_None? 0 : - (PyArray_IS_C_CONTIGUOUS(object) + (!PyArray_IS_F_CONTIGUOUS(object) ? ((object->nd == 1) ? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector) : object->dimensions[1]) : object->dimensions[0]), // COLUMN: If array is in row-major order: transpose (see README) (PyObject*)object == Py_None? 0 : - (PyArray_IS_C_CONTIGUOUS(object) + (!PyArray_IS_F_CONTIGUOUS(object) ? object->dimensions[0] : ((object->nd == 1) ? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector) - : object->dimensions[1]))) { + : object->dimensions[1]))), + object_(object) { if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object)) throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); + Py_XINCREF(object_); } Map &operator=(const Map &other) { - // Replace the memory that we point to (not a memory allocation) - new (this) Map(const_cast(other.data()), - other.rows(), - other.cols()); + if (other.object_) { + new (this) Map(other.object_); + } else { + // Replace the memory that we point to (not a memory allocation) + new (this) Map(const_cast(other.data()), + other.rows(), + other.cols()); + } + + return *this; + } + + Map &operator=(const MatrixType &other) { + MapBase::operator=(other); return *this; } @@ -440,7 +485,14 @@ public: operator MatrixType() const { return MatrixType(static_cast(*this)); - } + } + + virtual ~Map() { + Py_XDECREF(object_); + } + +private: + PyArrayObject * const object_; }; From f75c2f84273585ab2383681fc0cb98576eb81671 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 07:37:52 -0400 Subject: [PATCH 07/14] Cleaned up, corrected bearings --- cython/gtsam/examples/PlanarSLAMExample.py | 139 ++++++++------------- 1 file changed, 54 insertions(+), 85 deletions(-) diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index ca3d26bee..c55b13968 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -1,84 +1,65 @@ -import gtsam +""" +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) + +See LICENSE for the license information + +Simple robotics example using odometry measurements and bearing-range (laser) measurements +Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" + import numpy as np -import math + +from gtsam import (BearingRangeFactor2D, BetweenFactorPose2, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + Marginals, NonlinearFactorGraph, Point2, Pose2, + PriorFactorPose2, Rot2, Values, noiseModel_Diagonal, symbol) + +# Create noise models +PRIOR_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +ODOMETRY_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +MEASUREMENT_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) # Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() +graph = NonlinearFactorGraph() -# Create the keys we need for graph -# static Symbol x1('x',1), x2('x',2), x3('x',3); -# static Symbol l1('l',1), l2('l',2); -x1 = gtsam.symbol(ord('x'), 1) -x2 = gtsam.symbol(ord('x'), 2) -x3 = gtsam.symbol(ord('x'), 3) -l1 = gtsam.symbol(ord('l'), 4) -l2 = gtsam.symbol(ord('l'), 5) +# Create the keys corresponding to unknown variables in the factor graph +X1 = symbol(ord('x'), 1) +X2 = symbol(ord('x'), 2) +X3 = symbol(ord('x'), 3) +L1 = symbol(ord('l'), 4) +L2 = symbol(ord('l'), 5) -# Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) -# Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin -priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin -# noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta -priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -# graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph -graph.add(gtsam.PriorFactorPose2(x1, priorMean, priorNoise)) +# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model +graph.add(PriorFactorPose2(X1, Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) +# Add odometry factors between X1,X2 and X2,X3, respectively +graph.add(BetweenFactorPose2(X1, X2, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) +graph.add(BetweenFactorPose2(X2, X3, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) -# Add two odometry factors between x1,x2 and x2,x3 -# Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) -odometry = gtsam.Pose2(2.0, 0.0, 0.0) -# noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta -# 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.emplace_shared >(x1, x2, odometry, odometryNoise); -graph.add(gtsam.BetweenFactorPose2(x1, x2, odometry, odometryNoise)) -# graph.emplace_shared >(x2, x3, odometry, odometryNoise); -graph.add(gtsam.BetweenFactorPose2(x2, x3, odometry, odometryNoise)) - -# Add Range-Bearing measurements to two different landmarks -# create a noise model for the landmark measurements -# noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range -measurementNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) -# Rot2 bearing11 = Rot2::fromDegrees(45), -# bearing21 = Rot2::fromDegrees(90), -# bearing32 = Rot2::fromDegrees(90); -# double range11 = std::sqrt(4.0+4.0), -# range21 = 2.0, -# range32 = 2.0; -bearing11 = gtsam.Rot2.fromDegrees(np.pi/4) -bearing21 = gtsam.Rot2.fromDegrees(np.pi/2) -bearing32 = gtsam.Rot2.fromDegrees(np.pi/2) -range11 = np.sqrt(4.0+4.0) -range21 = 2.0 -range32 = 2.0 - -# Add Bearing-Range factors -# graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); -# graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); -# graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); -graph.add(gtsam.BearingRangeFactor2D(x1,l1,bearing11,range11,measurementNoise)) -graph.add(gtsam.BearingRangeFactor2D(x2,l1,bearing21,range21,measurementNoise)) -graph.add(gtsam.BearingRangeFactor2D(x3,l2,bearing32,range32,measurementNoise)) +# Add Range-Bearing measurements to two different landmarks L1 and L2 +graph.add(BearingRangeFactor2D( + X1, L1, Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) +graph.add(BearingRangeFactor2D( + X2, L1, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) +graph.add(BearingRangeFactor2D( + X3, L2, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) # Print graph -graph.print_("Factor Graph:\n"); +graph.print_("Factor Graph:\n") # Create (deliberately inaccurate) initial estimate -# Values initialEstimate; -# initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); -# initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); -# initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); -# initialEstimate.insert(l1, Point2(1.8, 2.1)); -# initialEstimate.insert(l2, Point2(4.1, 1.8)); -initialEstimate = gtsam.Values() -initialEstimate.insert(x1, gtsam.Pose2(-0.25, 0.20, 0.15)) -initialEstimate.insert(x2, gtsam.Pose2( 2.30, 0.10, -0.20)) -initialEstimate.insert(x3, gtsam.Pose2( 4.10, 0.10, 0.10)) -initialEstimate.insert(l1, gtsam.Point2( 1.80, 2.10)) -initialEstimate.insert(l2, gtsam.Point2( 4.10, 1.80)) +initial_estimate = Values() +initial_estimate.insert(X1, Pose2(-0.25, 0.20, 0.15)) +initial_estimate.insert(X2, Pose2(2.30, 0.10, -0.20)) +initial_estimate.insert(X3, Pose2(4.10, 0.10, 0.10)) +initial_estimate.insert(L1, Point2(1.80, 2.10)) +initial_estimate.insert(L2, Point2(4.10, 1.80)) # Print -initialEstimate.print_("Initial Estimate:\n"); +initial_estimate.print_("Initial Estimate:\n") # Optimize using Levenberg-Marquardt optimization. The optimizer # accepts an optional set of configuration parameters, controlling @@ -86,24 +67,12 @@ initialEstimate.print_("Initial Estimate:\n"); # to use, and the amount of information displayed during optimization. # Here we will use the default set of parameters. See the # documentation for the full set of parameters. -# LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); -# Values result = optimizer.optimize(); -# result.print("Final Result:\n"); -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate, params) +params = LevenbergMarquardtParams() +optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params) result = optimizer.optimize() result.print_("\nFinal Result:\n") # Calculate and print marginal covariances for all variables -# Marginals marginals(graph, result); -# print(marginals.marginalCovariance(x1), "x1 covariance"); -# print(marginals.marginalCovariance(x2), "x2 covariance"); -# print(marginals.marginalCovariance(x3), "x3 covariance"); -# print(marginals.marginalCovariance(l1), "l1 covariance"); -# print(marginals.marginalCovariance(l2), "l2 covariance"); -marginals = gtsam.Marginals(graph, result) -print("x1 covariance:\n", marginals.marginalCovariance(x1)) -print("x2 covariance:\n", marginals.marginalCovariance(x2)) -print("x3 covariance:\n", marginals.marginalCovariance(x3)) -print("x4 covariance:\n", marginals.marginalCovariance(l1)) -print("x5 covariance:\n", marginals.marginalCovariance(l2)) +marginals = Marginals(graph, result) +for (key, str) in [(X1,"X1"),(X2,"X2"),(X3,"X3"),(L1,"L1"),(L2,"L2")]: + print("{} covariance:\n{}\n".format(str,marginals.marginalCovariance(key))) From 025450dcc5a44996a91e157e9b8db2e6efda7adb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 08:06:59 -0400 Subject: [PATCH 08/14] Switched back to gtsam. pattern, added pylint exceptions --- cython/gtsam/examples/OdometryExample.py | 26 ++++++-- cython/gtsam/examples/PlanarSLAMExample.py | 68 +++++++++++---------- cython/gtsam/examples/Pose2SLAMExample.py | 46 ++++++++------ cython/gtsam/examples/SFMdata.py | 1 + cython/gtsam/examples/VisualISAM2Example.py | 14 ++++- 5 files changed, 97 insertions(+), 58 deletions(-) diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py index 3bdf6eda5..bfebcb45f 100644 --- a/cython/gtsam/examples/OdometryExample.py +++ b/cython/gtsam/examples/OdometryExample.py @@ -1,26 +1,40 @@ -#!/usr/bin/env python +""" +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) + +See LICENSE for the license information + +Simple robot motion example, with prior and two odometry measurements +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, E1101 + from __future__ import print_function import numpy as np import gtsam +# Create noise models +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + # 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)) +graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) # 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.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) graph.print_("\nFactor Graph:\n") # Create the data structure to hold the initialEstimate estimate to the solution diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index c55b13968..47c69e490 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -9,54 +9,56 @@ See LICENSE for the license information Simple robotics example using odometry measurements and bearing-range (laser) measurements Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function import numpy as np -from gtsam import (BearingRangeFactor2D, BetweenFactorPose2, - LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - Marginals, NonlinearFactorGraph, Point2, Pose2, - PriorFactorPose2, Rot2, Values, noiseModel_Diagonal, symbol) +import gtsam # Create noise models -PRIOR_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -ODOMETRY_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) -MEASUREMENT_NOISE = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) # Create an empty nonlinear factor graph -graph = NonlinearFactorGraph() +graph = gtsam.NonlinearFactorGraph() # Create the keys corresponding to unknown variables in the factor graph -X1 = symbol(ord('x'), 1) -X2 = symbol(ord('x'), 2) -X3 = symbol(ord('x'), 3) -L1 = symbol(ord('l'), 4) -L2 = symbol(ord('l'), 5) +X1 = gtsam.symbol(ord('x'), 1) +X2 = gtsam.symbol(ord('x'), 2) +X3 = gtsam.symbol(ord('x'), 3) +L1 = gtsam.symbol(ord('l'), 4) +L2 = gtsam.symbol(ord('l'), 5) # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model -graph.add(PriorFactorPose2(X1, Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) +graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) # Add odometry factors between X1,X2 and X2,X3, respectively -graph.add(BetweenFactorPose2(X1, X2, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) -graph.add(BetweenFactorPose2(X2, X3, Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) # Add Range-Bearing measurements to two different landmarks L1 and L2 -graph.add(BearingRangeFactor2D( - X1, L1, Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) -graph.add(BearingRangeFactor2D( - X2, L1, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) -graph.add(BearingRangeFactor2D( - X3, L2, Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) +graph.add(gtsam.BearingRangeFactor2D( + X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) +graph.add(gtsam.BearingRangeFactor2D( + X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) +graph.add(gtsam.BearingRangeFactor2D( + X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) # Print graph graph.print_("Factor Graph:\n") # Create (deliberately inaccurate) initial estimate -initial_estimate = Values() -initial_estimate.insert(X1, Pose2(-0.25, 0.20, 0.15)) -initial_estimate.insert(X2, Pose2(2.30, 0.10, -0.20)) -initial_estimate.insert(X3, Pose2(4.10, 0.10, 0.10)) -initial_estimate.insert(L1, Point2(1.80, 2.10)) -initial_estimate.insert(L2, Point2(4.10, 1.80)) +initial_estimate = gtsam.Values() +initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) +initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) +initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) +initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) +initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) # Print initial_estimate.print_("Initial Estimate:\n") @@ -67,12 +69,12 @@ initial_estimate.print_("Initial Estimate:\n") # to use, and the amount of information displayed during optimization. # Here we will use the default set of parameters. See the # documentation for the full set of parameters. -params = LevenbergMarquardtParams() -optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params) +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) result = optimizer.optimize() result.print_("\nFinal Result:\n") # Calculate and print marginal covariances for all variables -marginals = Marginals(graph, result) -for (key, str) in [(X1,"X1"),(X2,"X2"),(X3,"X3"),(L1,"L1"),(L2,"L2")]: - print("{} covariance:\n{}\n".format(str,marginals.marginalCovariance(key))) +marginals = gtsam.Marginals(graph, result) +for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: + print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index fe71788d8..f039b43e9 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -1,3 +1,16 @@ +""" +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) + +See LICENSE for the license information + +Simple robotics example using odometry measurements and bearing-range (laser) measurements +Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + from __future__ import print_function import math @@ -7,36 +20,38 @@ import numpy as np import gtsam -def Vector3(x, y, z): return np.array([x, y, z]) +def Vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=np.float) +# Create noise models +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) + # 1. Create a factor graph container and add factors to it graph = gtsam.NonlinearFactorGraph() # 2a. 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) -priorNoise = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) -graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise)) - -# For simplicity, we will use the same noise model for odometry and loop closures -model = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) +# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) +graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) # 2b. Add odometry factors # Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model)) +graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2, 0, math.pi / 2), model)) + 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2, 0, math.pi / 2), model)) + 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2, 0, math.pi / 2), model)) + 4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) # 2c. Add the loop closure constraint # This factor encodes the fact that we have returned to the same pose. In real # systems, these constraints may be identified in many ways, such as appearance-based # techniques with camera images. We will use another Between Factor to enforce this constraint: graph.add(gtsam.BetweenFactorPose2( - 5, 2, gtsam.Pose2(2, 0, math.pi / 2), model)) + 5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) graph.print_("\nFactor Graph:\n") # print # 3. Create the data structure to hold the initial_estimate estimate to the @@ -68,8 +83,5 @@ result.print_("Final Result:\n") # 5. Calculate and print marginal covariances for all variables marginals = gtsam.Marginals(graph, result) -print("x1 covariance:\n", marginals.marginalCovariance(1)) -print("x2 covariance:\n", marginals.marginalCovariance(2)) -print("x3 covariance:\n", marginals.marginalCovariance(3)) -print("x4 covariance:\n", marginals.marginalCovariance(4)) -print("x5 covariance:\n", marginals.marginalCovariance(5)) +for i in range(1, 6): + print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) diff --git a/cython/gtsam/examples/SFMdata.py b/cython/gtsam/examples/SFMdata.py index 742518c2c..1d5c499fa 100644 --- a/cython/gtsam/examples/SFMdata.py +++ b/cython/gtsam/examples/SFMdata.py @@ -3,6 +3,7 @@ A structure-from-motion example with landmarks - The landmarks form a 10 meter cube - The robot rotates around the landmarks, always facing towards the cube """ +# pylint: disable=invalid-name, E1101 import numpy as np diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py index 29383612d..b1bb911e0 100644 --- a/cython/gtsam/examples/VisualISAM2Example.py +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -1,5 +1,15 @@ -"""An example of running visual SLAM using iSAM2.""" -# pylint: disable=invalid-name +""" +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) + +See LICENSE for the license information + +An example of running visual SLAM using iSAM2. +Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 from __future__ import print_function From 5aa75b5594f21a1e321d776c4b3834df6a218a42 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 10:33:22 -0400 Subject: [PATCH 09/14] Remove -a, which created huge html files --- cmake/GtsamCythonWrap.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 6f378f77d..8736723d7 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -52,7 +52,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs) add_custom_command( OUTPUT ${generated_cpp} COMMAND - ${CYTHON_EXECUTABLE} -X boundscheck=False -a -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp} + ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp} VERBATIM) add_custom_target(${target} ALL DEPENDS ${generated_cpp}) endfunction() From 2f5a60d0393d7032d1c579b537c02a7c8090540f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 10:33:38 -0400 Subject: [PATCH 10/14] naming of local function --- cython/gtsam/examples/Pose2SLAMExample.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index f039b43e9..58006f2c8 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -20,14 +20,14 @@ import numpy as np import gtsam -def Vector3(x, y, z): +def vector3(x, y, z): """Create 3d double numpy array.""" return np.array([x, y, z], dtype=np.float) # Create noise models -PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) -ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) +ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) # 1. Create a factor graph container and add factors to it graph = gtsam.NonlinearFactorGraph() From c68d00fd550debe99d8e27116ad97fd559d7b8c1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 10:34:02 -0400 Subject: [PATCH 11/14] Added Scenario class --- gtsam.h | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index d0be06652..f885a93bd 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2509,7 +2509,7 @@ virtual class PreintegratedRotationParams { Matrix getGyroscopeCovariance() const; // TODO(frank): allow optional - // boost::optional getOmegaCoriolis() const; + // boost::optional getOmegaCoriolis() const; // boost::optional getBodyPSensor() const; }; @@ -2660,6 +2660,27 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ gtsam::Unit3 bRef() const; }; +#include +virtual class Scenario {}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(const Vector& w, const Vector& v); + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; +}; + +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + const Vector& v0, const Vector& a_n, + const Vector& omega_b); + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; +}; + //************************************************************************* // Utilities //************************************************************************* From f6696ce9e0e2071c64a04916740d8028e71eb490 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 10:59:51 -0400 Subject: [PATCH 12/14] Add named constructors for PreintegrationParams --- gtsam.h | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index f885a93bd..3622e520e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2502,6 +2502,11 @@ class NavState { #include virtual class PreintegratedRotationParams { PreintegratedRotationParams(); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); + void setGyroscopeCovariance(Matrix cov); void setOmegaCoriolis(Vector omega); void setBodyPSensor(const gtsam::Pose3& pose); @@ -2516,6 +2521,16 @@ virtual class PreintegratedRotationParams { #include virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); + + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); + void setAccelerometerCovariance(Matrix cov); void setIntegrationCovariance(Matrix cov); void setUse2ndOrderCoriolis(bool flag); @@ -2523,7 +2538,6 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { Matrix getAccelerometerCovariance() const; Matrix getIntegrationCovariance() const; bool getUse2ndOrderCoriolis() const; - void print(string s) const; }; #include From 295092faf672f142cfa2677535e87a7afbf381b1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 11:23:44 -0400 Subject: [PATCH 13/14] Added SimpleCamera LookAt version with default calibration --- gtsam.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam.h b/gtsam.h index 3622e520e..6e828486f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -938,6 +938,8 @@ virtual class SimpleCamera { static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector); // Testable void print(string s) const; From 16f8fb031a6d9c4c336adfecba462992e7f8a30c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 12:11:33 -0400 Subject: [PATCH 14/14] Updated copyright --- cython/gtsam/examples/OdometryExample.py | 2 +- cython/gtsam/examples/PlanarSLAMExample.py | 2 +- cython/gtsam/examples/Pose2SLAMExample.py | 2 +- cython/gtsam/examples/VisualISAM2Example.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py index bfebcb45f..0357aebb3 100644 --- a/cython/gtsam/examples/OdometryExample.py +++ b/cython/gtsam/examples/OdometryExample.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010, Georgia Tech Research Corporation, +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index 47c69e490..785faaea6 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010, Georgia Tech Research Corporation, +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index 58006f2c8..a9bb9d74b 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010, Georgia Tech Research Corporation, +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py index b1bb911e0..e2caee6d4 100644 --- a/cython/gtsam/examples/VisualISAM2Example.py +++ b/cython/gtsam/examples/VisualISAM2Example.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010, Georgia Tech Research Corporation, +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved Authors: Frank Dellaert, et al. (see THANKS for the full author list)