From 4ba7c59330cda33ec496ebcf1bdf6e7f97e13ffe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 27 Sep 2018 22:45:14 -0400 Subject: [PATCH 01/39] 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/39] 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/39] 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 285e2da5a808c6b88d2d0f389d0ba3d0bab6d159 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 23:55:49 -0400 Subject: [PATCH 04/39] Fixed lint errors --- gtsam/geometry/Unit3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 00edc1ad4..e0e0ecb56 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -257,7 +257,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From e1466b2609daa9ca06bfef9e1732a6f8a9dde540 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:45:42 -0400 Subject: [PATCH 05/39] Fixed uninitialized problem --- gtsam_unstable/slam/SmartRangeFactor.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5e107ea58..2170c8599 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,12 @@ #include #include #include + +#include #include +#include +#include +#include namespace gtsam { @@ -83,6 +88,7 @@ public: /** * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point + * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { //gttic_(triangulate); @@ -96,7 +102,7 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional best_circle; + boost::optional bestCircle2; // loop over all circles for(const Circle2& it: circles) { @@ -111,13 +117,14 @@ public: // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; - best_circle = it; + bestCircle2 = it; } } // use best fh to find actual intersection points + if (bestCircle2 && best_fh) { std::list intersections = circleCircleIntersection( - circle1.center, best_circle->center, best_fh); + circle1.center, bestCircle2->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; @@ -127,7 +134,10 @@ public: error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; - //gttoc_(triangulate); + } else { + throw std::runtime_error("triangulate failed"); + } + // gttoc_(triangulate); } /** From a34a9b8ff1512e62f738c6a276cd38204ec32f49 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:46:30 -0400 Subject: [PATCH 06/39] Fixed remaining lint errors --- gtsam_unstable/slam/SmartRangeFactor.h | 31 ++++++++++++-------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 2170c8599..3164b360e 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -28,8 +28,7 @@ namespace gtsam { * @addtogroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { -protected: - + protected: struct Circle2 { Circle2(const Point2& p, double r) : center(p), radius(r) { @@ -40,11 +39,10 @@ protected: typedef SmartRangeFactor This; - std::vector measurements_; ///< Range measurements - double variance_; ///< variance on noise - -public: + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise + public: /** Default constructor: don't use directly */ SmartRangeFactor() { } @@ -53,7 +51,7 @@ public: * Constructor * @param s standard deviation of range measurement noise */ - SmartRangeFactor(double s) : + explicit SmartRangeFactor(double s) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } @@ -91,7 +89,7 @@ public: * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - //gttic_(triangulate); + // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -105,11 +103,11 @@ public: boost::optional bestCircle2; // loop over all circles - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { // distance between circle centers. double d = distance2(circle1.center, it.center); if (d < 1e-9) - continue; // skip circles that are in the same location + continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); @@ -129,7 +127,7 @@ public: // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { error1 += distance2(it.center, p1); error2 += distance2(it.center, p2); } @@ -147,19 +145,20 @@ public: boost::optional&> H = boost::none) const { size_t n = size(); if (n < 3) { - if (H) + if (H) { // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) (*H)[j] = Matrix::Zero(3, 1); + } return Z_1x1; } else { Vector error = Z_1x1; // triangulate to get the optimized point - // TODO: Should we have a (better?) variant that does this in relative coordinates ? + // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(x); - // TODO: triangulation should be followed by an optimization given poses + // TODO(dellaert): triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); @@ -178,8 +177,6 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - }; - -} // \namespace gtsam +} // \namespace gtsam From 767c5d41ee8ef3914ce855997a984849702988ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 09:11:50 -0400 Subject: [PATCH 07/39] Showed compiler that B_ is always initialized --- gtsam/geometry/Unit3.cpp | 100 +++++++++++++++++++-------------------- 1 file changed, 49 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index e0e0ecb56..2c49f72e7 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -74,61 +74,59 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { tbb::mutex::scoped_lock lock(B_mutex_); #endif - // Return cached basis if available and the Jacobian isn't needed. if (B_ && !H) { + // Return cached basis if available and the Jacobian isn't needed. return *B_; - } - - // Return cached basis and derivatives if available. - if (B_ && H && H_B_) { + } else if (B_ && H && H_B_) { + // Return cached basis and derivatives if available. *H = *H_B_; + return *B_; + } else { + B_.reset(Matrix32()); + // Get the unit vector and derivative wrt this. + // NOTE(hayk): We can't call point3(), because it would recursively call basis(). + const Point3 n(p_); + + // Get the axis of rotation with the minimum projected length of the point + Point3 axis(0, 0, 1); + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + axis = Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + axis = Point3(0.0, 1.0, 0.0); + } + + // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with + // the chosen axis. + Matrix33 H_B1_n; + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix33 H_b1_B1; + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Matrix33 H_b2_n, H_b2_b1; + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. + (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + + if (H) { + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the derivative and fill the result. + H_B_.reset(Matrix62()); + (*H_B_) << H_b1_p, H_b2_p; + *H = *H_B_; + } + return *B_; } - - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); - if ((mx <= my) && (mx <= mz)) { - axis = Point3(1.0, 0.0, 0.0); - } else if ((my <= mx) && (my <= mz)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - B_.reset(Matrix32()); - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - - if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - - // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; - *H = *H_B_; - } - - return *B_; } /* ************************************************************************* */ @@ -257,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From b84f7ed22d221f5fd71fa382145bcd0cd7c692d8 Mon Sep 17 00:00:00 2001 From: Cyril Roussillon Date: Tue, 9 Oct 2018 17:30:41 +0200 Subject: [PATCH 08/39] (#378) Allow zero dimension factors, as it was in gtsam 3.x --- gtsam/linear/Scatter.cpp | 3 --- gtsam/nonlinear/internal/LevenbergMarquardtState.h | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index db5c25442..5312da34b 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -70,9 +70,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, iterator first = begin(); if (ordering) first += ordering->size(); if (first != end()) std::sort(first, end()); - - // Filter out keys with zero dimensions (if ordering had more keys) - erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index 19a390c45..bd5465dda 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -113,9 +113,9 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { // Small cache of A|b|model indexed by dimension. Can save many mallocs. mutable std::vector noiseModelCache; CachedModel* getCachedModel(size_t dim) const { - if (dim > noiseModelCache.size()) - noiseModelCache.resize(dim); - CachedModel* item = &noiseModelCache[dim - 1]; + if (dim >= noiseModelCache.size()) + noiseModelCache.resize(dim+1); + CachedModel* item = &noiseModelCache[dim]; if (!item->model) *item = CachedModel(dim, 1.0 / std::sqrt(lambda)); return item; From 6ec0ca798299a001f6cb8c87066c4275d27d71d4 Mon Sep 17 00:00:00 2001 From: Andrei Costinescu Date: Fri, 12 Oct 2018 10:32:41 +0000 Subject: [PATCH 09/39] Fixed maybe-uninitialized warnings in Unit3::basis function --- gtsam/geometry/Unit3.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 2c49f72e7..21f66604e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -82,7 +82,6 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { *H = *H_B_; return *B_; } else { - B_.reset(Matrix32()); // Get the unit vector and derivative wrt this. // NOTE(hayk): We can't call point3(), because it would recursively call basis(). const Point3 n(p_); @@ -111,7 +110,9 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); // Create the basis by stacking b1 and b2. - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + Matrix32 stacked; + stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + B_.reset(stacked); if (H) { // Chain rule tomfoolery to compute the derivative. @@ -120,8 +121,9 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; + Matrix62 derivative; + derivative << H_b1_p, H_b2_p; + H_B_.reset(derivative); *H = *H_B_; } From 6c09d8681c73cd3b21ab1bb8e587bd1633e777b9 Mon Sep 17 00:00:00 2001 From: AndreiCostinescu Date: Fri, 12 Oct 2018 19:10:18 -0400 Subject: [PATCH 10/39] Fixed warning in SmartRangeFactor.h --- gtsam_unstable/slam/SmartRangeFactor.h | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 3164b360e..08f6bf6ce 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -121,17 +121,18 @@ class SmartRangeFactor: public NoiseModelFactor { // use best fh to find actual intersection points if (bestCircle2 && best_fh) { - std::list intersections = circleCircleIntersection( - circle1.center, bestCircle2->center, best_fh); + auto bestCircleCenter = bestCircle2->center; + std::list intersections = circleCircleIntersection( + circle1.center, bestCircleCenter, best_fh); - // pick winner based on other measurements - double error1 = 0, error2 = 0; - Point2 p1 = intersections.front(), p2 = intersections.back(); - for (const Circle2& it : circles) { - error1 += distance2(it.center, p1); - error2 += distance2(it.center, p2); - } - return (error1 < error2) ? p1 : p2; + // pick winner based on other measurements + double error1 = 0, error2 = 0; + Point2 p1 = intersections.front(), p2 = intersections.back(); + for (const Circle2& it : circles) { + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); + } + return (error1 < error2) ? p1 : p2; } else { throw std::runtime_error("triangulate failed"); } From 540be68b803aac74be823933075d9d09571fd429 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Oct 2018 23:30:47 -0400 Subject: [PATCH 11/39] Refactored code paths to cover all 8 cases of H, B_, H_B_ with minimal calculation. Previous version was a bit hard to parse. Assign directly to B (formerly stacked) and jacobian (formerly derivative). --- gtsam/geometry/Unit3.cpp | 89 +++++++++++++++++++++------------------- 1 file changed, 47 insertions(+), 42 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 21f66604e..df1152487 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -68,67 +68,72 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I - // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or - // there is still a latent bug to watch out for. + // NOTE(hayk): At some point it seemed like this reproducably resulted in + // deadlock. However, I can't see reason why and I can no longer reproduce it. + // It may have been a red herring, or there is still a latent bug. tbb::mutex::scoped_lock lock(B_mutex_); #endif - - if (B_ && !H) { - // Return cached basis if available and the Jacobian isn't needed. - return *B_; - } else if (B_ && H && H_B_) { - // Return cached basis and derivatives if available. - *H = *H_B_; - return *B_; - } else { - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); + Point3 n, axis; + if (!B_ || (H && !H_B_)) { + // Get the unit vector + // NOTE(hayk): can't call point3(), because would recursively call basis(). + n = Point3(p_); // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); + axis = Point3(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); } + } - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + if (H) { + if (!H_B_) { + // Compute Jacobian. Possibly recomputes B_ - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + Matrix33 H_B1_n; + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix32 B; + Matrix33 H_b1_B1; + B.col(0) = normalize(B1, &H_b1_B1); - // Create the basis by stacking b1 and b2. - Matrix32 stacked; - stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - B_.reset(stacked); + // Get the second basis vector b2, which is orthogonal to n and b1. + Matrix33 H_b2_n, H_b2_b1; + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); - if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + // Chain rule tomfoolery to compute the jacobian. + Matrix62 jacobian; + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - // Cache the derivative and fill the result. - Matrix62 derivative; - derivative << H_b1_p, H_b2_p; - H_B_.reset(derivative); - *H = *H_B_; + // Cache the result and jacobian + B_.reset(B); + H_B_.reset(jacobian); } - return *B_; + // Return cached jacobian, possibly computed just above + *H = *H_B_; } + + if (!B_) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + + return *B_; } /* ************************************************************************* */ From 88c4bd0a330043f4c4b3f50ced55fd793b6f3c48 Mon Sep 17 00:00:00 2001 From: Andrei Costinescu Date: Sat, 13 Oct 2018 06:56:04 -0400 Subject: [PATCH 12/39] Second attempt at a logical refactor of Unit3::basis method --- gtsam/geometry/Unit3.cpp | 83 ++++++++++++++++++---------------------- 1 file changed, 38 insertions(+), 45 deletions(-) mode change 100644 => 100755 gtsam/geometry/Unit3.cpp diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp old mode 100644 new mode 100755 index 61bcbc457..d5ddc115b --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -69,70 +69,63 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB // NOTE(hayk): At some point it seemed like this reproducably resulted in - // deadlock. However, I can't see reason why and I can no longer reproduce it. - // It may have been a red herring, or there is still a latent bug. + // deadlock. However, I don't know why and I can no longer reproduce it. + // It either was a red herring or there is still a latent bug left to debug. tbb::mutex::scoped_lock lock(B_mutex_); #endif - Point3 n, axis; - if (!B_ || (H && !H_B_)) { + + bool cachedBasis = static_cast(B_); + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; + + if (!cachedBasis) { // Get the unit vector - // NOTE(hayk): can't call point3(), because would recursively call basis(). - n = Point3(p_); + // NOTE(hayk): We can't call point3(), due to the recursive call of basis(). + const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - axis = Point3(0, 0, 1); + Point3 axis(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); } + + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, through the cross-product of n and b1. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Point3 b2 = + gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. + Matrix32 stacked; + stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + B_.reset(stacked); } if (H) { - if (!H_B_) { - // Compute Jacobian. Possibly recomputes B_ + if (!cachedBasis || !H_B_) { + // If Jacobian not cached or the basis was not cached, recompute it. + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - // Choose the direction of the first basis vector b1 in the tangent plane - // by crossing n with the chosen axis. - Matrix33 H_B1_n; - const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix32 B; - Matrix33 H_b1_B1; - B.col(0) = normalize(B1, &H_b1_B1); - - // Get the second basis vector b2, which is orthogonal to n and b1. - Matrix33 H_b2_n, H_b2_b1; - B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); - - // Chain rule tomfoolery to compute the jacobian. - Matrix62 jacobian; - const Matrix32& H_n_p = B; - jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; - auto H_b1_p = jacobian.block<3, 2>(0, 0); - jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - - // Cache the result and jacobian - B_.reset(B); - H_B_.reset(jacobian); + // Cache the derivative and fill the result. + Matrix62 derivative; + derivative << H_b1_p, H_b2_p; + H_B_.reset(derivative); } - // Return cached jacobian, possibly computed just above *H = *H_B_; } - if (!B_) { - // Same calculation as above, without derivatives. - // Done after H block, as that possibly computes B_ for the first time - Matrix32 B; - const Point3 B1 = gtsam::cross(n, axis); - B.col(0) = normalize(B1); - B.col(1) = gtsam::cross(n, B.col(0)); - B_.reset(B); - } - return *B_; } @@ -262,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From 35d4bb1a7618aa70c7aeccbc31998f85699bdaf3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 12:18:25 -0400 Subject: [PATCH 13/39] After discussion with Andrei, a final version of logic, and new tests to check more cases. Tested with both typedef and old Point3 configs. --- gtsam/geometry/Unit3.cpp | 97 ++++++++++++++++-------------- gtsam/geometry/tests/testUnit3.cpp | 24 +++++--- 2 files changed, 70 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index d5ddc115b..08f14c829 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -65,6 +65,19 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return Unit3(d[0], d[1], d[2]); } +/* ************************************************************************* */ +// Get the axis of rotation with the minimum projected length of the point +static Point3 CalculateBestAxis(const Point3& n) { + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + return Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + return Point3(0.0, 1.0, 0.0); + } else { + return Point3(0, 0, 1); + } +} + /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB @@ -74,58 +87,54 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { tbb::mutex::scoped_lock lock(B_mutex_); #endif - bool cachedBasis = static_cast(B_); - Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - - if (!cachedBasis) { - // Get the unit vector - // NOTE(hayk): We can't call point3(), due to the recursive call of basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); - if ((mx <= my) && (mx <= mz)) { - axis = Point3(1.0, 0.0, 0.0); - } else if ((my <= mx) && (my <= mz)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane - // by crossing n with the chosen axis. - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, through the cross-product of n and b1. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Point3 b2 = - gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - Matrix32 stacked; - stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - B_.reset(stacked); - } + const bool cachedBasis = static_cast(B_); + const bool cachedJacobian = static_cast(H_B_); if (H) { - if (!cachedBasis || !H_B_) { - // If Jacobian not cached or the basis was not cached, recompute it. - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + if (!cachedJacobian) { + // Compute Jacobian. Recomputes B_ + Matrix32 B; + Matrix62 jacobian; + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - // Cache the derivative and fill the result. - Matrix62 derivative; - derivative << H_b1_p, H_b2_p; - H_B_.reset(derivative); + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + B.col(0) = normalize(B1, &H_b1_B1); + + // Get the second basis vector b2, which is orthogonal to n and b1. + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); + + // Chain rule tomfoolery to compute the jacobian. + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the result and jacobian + H_B_.reset(jacobian); + B_.reset(B); } + // Return cached jacobian, possibly computed just above *H = *H_B_; } + if (!cachedBasis) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + return *B_; } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 71093d668..542aca038 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -314,15 +314,24 @@ TEST(Unit3, basis) { Unit3 p(0.1, -0.2, 0.9); Matrix expected(3, 2); - expected << 0.0, -0.994169047, 0.97618706, - -0.0233922129, 0.216930458, 0.105264958; + expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958; Matrix62 actualH; - Matrix actual = p.basis(actualH); - EXPECT(assert_equal(expected, actual, 1e-6)); - Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + boost::bind(BasisTest, _1, boost::none), p); + + // without H, first time + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // without H, cached + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // with H, first time + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + + // with H, cached + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } @@ -432,7 +441,8 @@ TEST(Unit3, ErrorBetweenFactor) { // Add process factors using the dot product error function. SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); for (size_t i = 0; i < data.size() - 1; i++) { - Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + Expression exp(Expression(U(i)), &Unit3::errorVector, + Expression(U(i + 1))); graph.addExpressionFactor(R_process, Vector2::Zero(), exp); } From c428e307843561f8188ee5be06dab1b0a0dc8f64 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 15:29:07 -0400 Subject: [PATCH 14/39] Example due to Robert Truax in Issue #280 --- examples/ImuFactorExample2.cpp | 171 +++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) create mode 100644 examples/ImuFactorExample2.cpp diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp new file mode 100644 index 000000000..059c72d83 --- /dev/null +++ b/examples/ImuFactorExample2.cpp @@ -0,0 +1,171 @@ + +#include +#include +#include +#include +#include +#include +#include + +#define GTSAM4 + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +std::vector createPoses() { + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 80; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); + } + return poses; +} +/* ************************************************************************* */ + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Create the set of ground-truth landmarks and poses + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph newgraph; + NonlinearFactorGraph totalgraph; + + // Create ISAM2 solver + ISAM2 isam, isam_full; + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate, totalEstimate, result; + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + newgraph.push_back(PriorFactor(0, poses[0], noise)); + totalgraph.push_back(PriorFactor(0, poses[0], noise)); + + // Add imu priors + int biasidx = 1000; + noiseModel::Diagonal::shared_ptr biasnoise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); + PriorFactor biasprior(biasidx,imuBias::ConstantBias(),biasnoise); + newgraph.push_back(biasprior); + totalgraph.push_back(biasprior); + initialEstimate.insert(biasidx, imuBias::ConstantBias()); + totalEstimate.insert(biasidx, imuBias::ConstantBias()); + noiseModel::Diagonal::shared_ptr velnoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); +#ifdef GTSAM4 + PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); +#else + PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); +#endif + newgraph.push_back(velprior); + totalgraph.push_back(velprior); + +#ifdef GTSAM4 + initialEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); + totalEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); +#else + initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); + totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); +#endif + + Matrix3 I; + I << 1, 0, 0, 0, 1, 0, 0, 0, 1; + Matrix3 accCov = I*0.1; + Matrix3 gyroCov = I*0.1; + Matrix3 intCov = I*0.1; + bool secOrder = false; +#ifdef GTSAM4 + // IMU preintegrator + PreintegratedImuMeasurements accum(PreintegrationParams::MakeSharedD()); + accum.params()->setAccelerometerCovariance(accCov); + accum.params()->setGyroscopeCovariance(gyroCov); + accum.params()->setIntegrationCovariance(intCov); + accum.params()->setUse2ndOrderCoriolis(secOrder); + accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); +#else + ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder); +#endif + + // Simulate poses and imu measurements, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { +#ifdef GTSAM4 + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); +#else + Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); +#endif + if (i == 0) { // First time add two poses + initialEstimate.insert(0, poses[0].compose(delta)); + initialEstimate.insert(1, poses[1].compose(delta)); + totalEstimate.insert(0, poses[0].compose(delta)); + totalEstimate.insert(1, poses[1].compose(delta)); + } else if (i >= 2) { // Add more poses as necessary + initialEstimate.insert(i, poses[i].compose(delta)); + totalEstimate.insert(i, poses[i].compose(delta)); + } + + if (i > 0) { + // Add Bias variables periodically + if (i % 5 == 0) { + biasidx++; + Symbol b1 = biasidx-1; + Symbol b2 = biasidx; + imuBias::ConstantBias basebias = imuBias::ConstantBias(); + Vector6 covvec; + covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; + noiseModel::Diagonal::shared_ptr cov = noiseModel::Diagonal::Variances(covvec); + Vector6 zerovec; + zerovec << 0, 0, 0, 0, 0, 0; + BetweenFactor::shared_ptr f(new BetweenFactor(b1, b2, imuBias::ConstantBias(), cov)); + newgraph.add(f); + totalgraph.add(f); + initialEstimate.insert(biasidx, imuBias::ConstantBias()); + totalEstimate.insert(biasidx, imuBias::ConstantBias()); + } + // Add Imu Factor + accum.integrateMeasurement((Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0, 0, 0).finished(), 0.5); +#ifdef GTSAM4 + ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum); +#else + ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum, (Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0.0, 0.0, 0.0).finished()); +#endif + + newgraph.add(imufac); + totalgraph.add(imufac); +#ifdef GTSAM4 + initialEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity + totalEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity +#else + initialEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity + totalEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity +#endif + accum.resetIntegration(); + } + + // Batch solution + isam_full = ISAM2(); + isam_full.update(totalgraph, totalEstimate); + result = isam_full.calculateEstimate(); + + // Incremental solution + isam.update(newgraph, initialEstimate); + result = isam.calculateEstimate(); + newgraph = NonlinearFactorGraph(); + initialEstimate.clear(); + } + + return 0; +} +/* ************************************************************************* */ + From 18234f68fd0a694e0d14c07499fc910bbd06ffff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 16:25:58 -0400 Subject: [PATCH 15/39] Cleanup, c++11 --- examples/ImuFactorExample2.cpp | 96 ++++++++++++++++++---------------- 1 file changed, 51 insertions(+), 45 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 059c72d83..1d6b32685 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,11 +1,13 @@ -#include -#include +#include +#include #include #include +#include #include -#include -#include +#include + +#include #define GTSAM4 @@ -13,36 +15,32 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -std::vector createPoses() { - +vector createPoses() { // Create the set of ground-truth poses - std::vector poses; + vector poses; double radius = 30.0; int i = 0; double theta = 0.0; - gtsam::Point3 up(0,0,1); - gtsam::Point3 target(0,0,0); - for(; i < 80; ++i, theta += 2*M_PI/8) { - gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); - gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); + Point3 up(0, 0, 1); + Point3 target(0, 0, 0); + for (; i < 80; ++i, theta += 2 * M_PI / 8) { + Point3 position(radius * cos(theta), radius * sin(theta), 0.0); + SimpleCamera camera = SimpleCamera::Lookat(position, target, up); poses.push_back(camera.pose()); } return poses; } -/* ************************************************************************* */ /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Create the set of ground-truth landmarks and poses vector poses = createPoses(); // Create a factor graph - NonlinearFactorGraph newgraph; - NonlinearFactorGraph totalgraph; + NonlinearFactorGraph newgraph, totalgraph; - // Create ISAM2 solver - ISAM2 isam, isam_full; + // Create (incremental) ISAM2 solver + ISAM2 isam; // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth @@ -50,41 +48,46 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); newgraph.push_back(PriorFactor(0, poses[0], noise)); totalgraph.push_back(PriorFactor(0, poses[0], noise)); // Add imu priors int biasidx = 1000; - noiseModel::Diagonal::shared_ptr biasnoise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); - PriorFactor biasprior(biasidx,imuBias::ConstantBias(),biasnoise); + auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); + PriorFactor biasprior(biasidx, imuBias::ConstantBias(), + biasnoise); newgraph.push_back(biasprior); totalgraph.push_back(biasprior); initialEstimate.insert(biasidx, imuBias::ConstantBias()); totalEstimate.insert(biasidx, imuBias::ConstantBias()); - noiseModel::Diagonal::shared_ptr velnoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + + Vector gravity(3), zero(3); + gravity << 0, 0, -9.8; + zero << 0, 0, 0; #ifdef GTSAM4 - PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); + PriorFactor velprior(100, zero, velnoise); #else - PriorFactor velprior(100,(Vector(3) << 0, 0, 0).finished(), velnoise); + PriorFactor velprior(100, zero, velnoise); #endif newgraph.push_back(velprior); totalgraph.push_back(velprior); #ifdef GTSAM4 - initialEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); - totalEstimate.insert(100, (Vector(3) << 0, 0, 0).finished()); + initialEstimate.insert(100, zero); + totalEstimate.insert(100, zero); #else - initialEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); - totalEstimate.insert(100, LieVector((Vector(3) << 0, 0, 0).finished())); + initialEstimate.insert(100, LieVector(zero)); + totalEstimate.insert(100, LieVector(zero)); #endif Matrix3 I; I << 1, 0, 0, 0, 1, 0, 0, 0, 1; - Matrix3 accCov = I*0.1; - Matrix3 gyroCov = I*0.1; - Matrix3 intCov = I*0.1; + Matrix3 accCov = I * 0.1; + Matrix3 gyroCov = I * 0.1; + Matrix3 intCov = I * 0.1; bool secOrder = false; #ifdef GTSAM4 // IMU preintegrator @@ -95,7 +98,8 @@ int main(int argc, char* argv[]) { accum.params()->setUse2ndOrderCoriolis(secOrder); accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); #else - ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, gyroCov, intCov, secOrder); + ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, + gyroCov, intCov, secOrder); #endif // Simulate poses and imu measurements, adding them to the factor graph @@ -119,42 +123,45 @@ int main(int argc, char* argv[]) { // Add Bias variables periodically if (i % 5 == 0) { biasidx++; - Symbol b1 = biasidx-1; + Symbol b1 = biasidx - 1; Symbol b2 = biasidx; - imuBias::ConstantBias basebias = imuBias::ConstantBias(); Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; - noiseModel::Diagonal::shared_ptr cov = noiseModel::Diagonal::Variances(covvec); + auto cov = noiseModel::Diagonal::Variances(covvec); Vector6 zerovec; zerovec << 0, 0, 0, 0, 0, 0; - BetweenFactor::shared_ptr f(new BetweenFactor(b1, b2, imuBias::ConstantBias(), cov)); + auto f = boost::make_shared >( + b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); totalgraph.add(f); initialEstimate.insert(biasidx, imuBias::ConstantBias()); totalEstimate.insert(biasidx, imuBias::ConstantBias()); } // Add Imu Factor - accum.integrateMeasurement((Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0, 0, 0).finished(), 0.5); + accum.integrateMeasurement(gravity, zero, 0.5); #ifdef GTSAM4 - ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum); + ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum); #else - ImuFactor imufac(i-1, 100+i-1,i,100+i, biasidx, accum, (Vector(3) << 0.0, 0.0, -9.8).finished(), (Vector(3) << 0.0, 0.0, 0.0).finished()); + ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum, gravity, + zero); #endif newgraph.add(imufac); totalgraph.add(imufac); + + // insert new velocity #ifdef GTSAM4 - initialEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity - totalEstimate.insert(100+i, (Vector(3) << 0.0, 0.0, -9.8).finished()); // insert new velocity + initialEstimate.insert(100 + i, gravity); + totalEstimate.insert(100 + i, gravity); #else - initialEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity - totalEstimate.insert(100+i, LieVector((Vector(3) << 0.0, 0.0, -9.8).finished())); // insert new velocity + initialEstimate.insert(100 + i, LieVector(gravity)); + totalEstimate.insert(100 + i, LieVector(gravity)); #endif accum.resetIntegration(); } // Batch solution - isam_full = ISAM2(); + ISAM2 isam_full; isam_full.update(totalgraph, totalEstimate); result = isam_full.calculateEstimate(); @@ -168,4 +175,3 @@ int main(int argc, char* argv[]) { return 0; } /* ************************************************************************* */ - From eb447d28a11aa00a34942e6a1a6b966dbc200608 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 17:48:36 -0400 Subject: [PATCH 16/39] Added symbol keys --- examples/ImuFactorExample2.cpp | 75 +++++++++++++++++----------------- 1 file changed, 38 insertions(+), 37 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 1d6b32685..97002eb2a 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -18,12 +18,9 @@ using namespace gtsam; vector createPoses() { // Create the set of ground-truth poses vector poses; - double radius = 30.0; - int i = 0; - double theta = 0.0; - Point3 up(0, 0, 1); - Point3 target(0, 0, 0); - for (; i < 80; ++i, theta += 2 * M_PI / 8) { + double radius = 30.0, theta = 0.0; + Point3 up(0, 0, 1), target(0, 0, 0); + for (size_t i = 0; i < 80; ++i, theta += 2 * M_PI / 8) { Point3 position(radius * cos(theta), radius * sin(theta), 0.0); SimpleCamera camera = SimpleCamera::Lookat(position, target, up); poses.push_back(camera.pose()); @@ -33,6 +30,10 @@ vector createPoses() { /* ************************************************************************* */ int main(int argc, char* argv[]) { + // Shorthand for velocity and pose variables + using symbol_shorthand::V; + using symbol_shorthand::X; + // Create the set of ground-truth landmarks and poses vector poses = createPoses(); @@ -50,37 +51,38 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - newgraph.push_back(PriorFactor(0, poses[0], noise)); - totalgraph.push_back(PriorFactor(0, poses[0], noise)); + newgraph.push_back(PriorFactor(X(0), poses[0], noise)); + totalgraph.push_back(PriorFactor(X(0), poses[0], noise)); // Add imu priors - int biasidx = 1000; + Key biasKey = Symbol('b', 0); auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); - PriorFactor biasprior(biasidx, imuBias::ConstantBias(), + PriorFactor biasprior(biasKey, imuBias::ConstantBias(), biasnoise); newgraph.push_back(biasprior); totalgraph.push_back(biasprior); - initialEstimate.insert(biasidx, imuBias::ConstantBias()); - totalEstimate.insert(biasidx, imuBias::ConstantBias()); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + totalEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - Vector gravity(3), zero(3); + Vector gravity(3), zero(3), velocity(3); gravity << 0, 0, -9.8; zero << 0, 0, 0; + velocity << 0, 0, 0; #ifdef GTSAM4 - PriorFactor velprior(100, zero, velnoise); + PriorFactor velprior(V(0), zero, velnoise); #else - PriorFactor velprior(100, zero, velnoise); + PriorFactor velprior(V(0), zero, velnoise); #endif newgraph.push_back(velprior); totalgraph.push_back(velprior); #ifdef GTSAM4 - initialEstimate.insert(100, zero); - totalEstimate.insert(100, zero); + initialEstimate.insert(V(0), zero); + totalEstimate.insert(V(0), zero); #else - initialEstimate.insert(100, LieVector(zero)); - totalEstimate.insert(100, LieVector(zero)); + initialEstimate.insert(V(0), LieVector(zero)); + totalEstimate.insert(V(0), LieVector(zero)); #endif Matrix3 I; @@ -110,21 +112,21 @@ int main(int argc, char* argv[]) { Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); #endif if (i == 0) { // First time add two poses - initialEstimate.insert(0, poses[0].compose(delta)); - initialEstimate.insert(1, poses[1].compose(delta)); - totalEstimate.insert(0, poses[0].compose(delta)); - totalEstimate.insert(1, poses[1].compose(delta)); + initialEstimate.insert(X(0), poses[0].compose(delta)); + initialEstimate.insert(X(1), poses[1].compose(delta)); + totalEstimate.insert(X(0), poses[0].compose(delta)); + totalEstimate.insert(X(1), poses[1].compose(delta)); } else if (i >= 2) { // Add more poses as necessary - initialEstimate.insert(i, poses[i].compose(delta)); - totalEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.insert(X(i), poses[i].compose(delta)); + totalEstimate.insert(X(i), poses[i].compose(delta)); } if (i > 0) { // Add Bias variables periodically if (i % 5 == 0) { - biasidx++; - Symbol b1 = biasidx - 1; - Symbol b2 = biasidx; + biasKey++; + Symbol b1 = biasKey - 1; + Symbol b2 = biasKey; Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; auto cov = noiseModel::Diagonal::Variances(covvec); @@ -134,28 +136,27 @@ int main(int argc, char* argv[]) { b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); totalgraph.add(f); - initialEstimate.insert(biasidx, imuBias::ConstantBias()); - totalEstimate.insert(biasidx, imuBias::ConstantBias()); + initialEstimate.insert(biasKey, imuBias::ConstantBias()); + totalEstimate.insert(biasKey, imuBias::ConstantBias()); } // Add Imu Factor accum.integrateMeasurement(gravity, zero, 0.5); #ifdef GTSAM4 - ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum); + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); #else - ImuFactor imufac(i - 1, 100 + i - 1, i, 100 + i, biasidx, accum, gravity, + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity, zero); #endif - newgraph.add(imufac); totalgraph.add(imufac); // insert new velocity #ifdef GTSAM4 - initialEstimate.insert(100 + i, gravity); - totalEstimate.insert(100 + i, gravity); + initialEstimate.insert(V(i), velocity); + totalEstimate.insert(V(i), velocity); #else - initialEstimate.insert(100 + i, LieVector(gravity)); - totalEstimate.insert(100 + i, LieVector(gravity)); + initialEstimate.insert(V(i), LieVector(velocity)); + totalEstimate.insert(V(i), LieVector(velocity)); #endif accum.resetIntegration(); } From 650b65f1fb69d9eacb090d597b1bb02f7735f7cb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 18:43:06 -0400 Subject: [PATCH 17/39] 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 18/39] 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 19/39] 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 20/39] 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 21/39] 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 22/39] 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 23/39] 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 24/39] 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 25/39] 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 26/39] 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 27/39] 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) From 97a5d5a64a97d13320defc5330f7ea35abfc8b4e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 14:53:20 -0400 Subject: [PATCH 28/39] Python example with reasonable measurements, in body frame. Still a TODO left. --- cython/gtsam/examples/ImuFactorExample2.py | 207 +++++++++++++++++++++ 1 file changed, 207 insertions(+) create mode 100644 cython/gtsam/examples/ImuFactorExample2.py diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py new file mode 100644 index 000000000..41b06607b --- /dev/null +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -0,0 +1,207 @@ +""" +iSAM2 example with ImuFactor. +Author: Robert Truax (C++), Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import math + +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 + + +def X(key): + """Create symbol for pose key.""" + return gtsam.symbol(ord('x'), key) + + +def V(key): + """Create symbol for velocity key.""" + return gtsam.symbol(ord('v'), key) + + +def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=np.float) + + +def create_poses(angular_velocity=np.pi, + delta_t=0.01, radius=30.0): + # Create the set of ground-truth poses + poses = [] + theta = 0.0 + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + for i in range(80): + position = gtsam.Point3(radius * math.cos(theta), + radius * math.sin(theta), 0.0) + camera = gtsam.SimpleCamera.Lookat( + position, target, up, gtsam.Cal3_S2()) + poses.append(camera.pose()) + theta += delta_t * angular_velocity + + return poses + + +def ISAM2_plot(values): + """Plot poses.""" + + # Declare an id for the figure + fignum = 0 + + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plt.cla() + + i = 0 + min_bounds = 0, 0, 0 + max_bounds = 0, 0, 0 + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + gtsam_plot.plot_pose3(fignum, pose_i, 10) + position = pose_i.translation().vector() + min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)] + max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)] + # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0 + i += 1 + + # draw + axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1) + axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1) + axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1) + plt.pause(1) + + +I = np.eye(3) +accCov = I * 0.1 +gyroCov = I * 0.1 +intCov = I * 0.1 +secOrder = False + +# IMU preintegration parameters +# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis +g = 9.81 +PARAMS = gtsam.PreintegrationParams.MakeSharedU() +PARAMS.setAccelerometerCovariance(accCov) +PARAMS.setGyroscopeCovariance(gyroCov) +PARAMS.setIntegrationCovariance(intCov) +PARAMS.setUse2ndOrderCoriolis(secOrder) +PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) + + +def IMU_example(): + + # Create the set of ground-truth landmarks and poses + angular_velocity = math.radians(180) # rad/sec + delta_t = 1.0/18 # makes for 10 degrees per step + radius = 30 + poses = create_poses(angular_velocity, delta_t, radius) + + # Create a factor graph + newgraph = gtsam.NonlinearFactorGraph() + totalgraph = gtsam.NonlinearFactorGraph() + + # Create (incremental) ISAM2 solver + isam = gtsam.ISAM2() + + # Create the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initialEstimate = gtsam.Values() + totalEstimate = gtsam.Values() + + # Add a prior on pose x0. This indirectly specifies where the origin is. + # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + totalgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + + # Add imu priors + biasKey = gtsam.symbol(ord('b'), 0) + biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), + biasnoise) + newgraph.push_back(biasprior) + totalgraph.push_back(biasprior) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + + # Calculate with correct initial velocity + velocity = vector3(0, angular_velocity * radius, 0) + velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) + newgraph.push_back(velprior) + totalgraph.push_back(velprior) + initialEstimate.insert(V(0), velocity) + totalEstimate.insert(V(0), velocity) + + accum = gtsam.PreintegratedImuMeasurements(PARAMS) + + # Simulate poses and imu measurements, adding them to the factor graph + for i, pose_i in enumerate(poses): + delta = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), + gtsam.Point3(0.05, -0.10, 0.20)) + if i == 0: # First time add two poses + initialEstimate.insert(X(0), poses[0].compose(delta)) + initialEstimate.insert(X(1), poses[1].compose(delta)) + totalEstimate.insert(X(0), poses[0].compose(delta)) + totalEstimate.insert(X(1), poses[1].compose(delta)) + elif i >= 2: # Add more poses as necessary + initialEstimate.insert(X(i), pose_i.compose(delta)) + totalEstimate.insert(X(i), pose_i.compose(delta)) + + if i > 0: + # Add Bias variables periodically + if i % 5 == 0: + biasKey += 1 + b1 = biasKey - 1 + b2 = biasKey + cov = gtsam.noiseModel_Isotropic.Variance(6, 0.1) + f = gtsam.BetweenFactorConstantBias( + b1, b2, gtsam.imuBias_ConstantBias(), cov) + newgraph.add(f) + totalgraph.add(f) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + + # Predict acceleration and gyro measurements in (actual) body frame + # TODO: calculate correct acceleration due to circular trajectory + nRb = pose_i.rotation().matrix() + bRn = np.transpose(nRb) + measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) + + # Add Imu Factor + imufac = gtsam.ImuFactor( + X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + newgraph.add(imufac) + totalgraph.add(imufac) + + # insert new velocity + initialEstimate.insert(V(i), velocity) + totalEstimate.insert(V(i), velocity) + accum.resetIntegration() + + # Batch solution + isam_full = gtsam.ISAM2() + isam_full.update(totalgraph, totalEstimate) + result = isam_full.calculateEstimate() + + # Incremental solution + isam.update(newgraph, initialEstimate) + result = isam.calculateEstimate() + newgraph = gtsam.NonlinearFactorGraph() + initialEstimate.clear() + + # ISAM2_plot(result) + + +if __name__ == '__main__': + IMU_example() From 89101c12202f71141416a36556c7909ff94998ce Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 10:36:13 -0400 Subject: [PATCH 29/39] Addressed (some) comments by Duy --- cython/gtsam/examples/__init__.py | 1 - cython/gtsam/utils/plot.py | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cython/gtsam/examples/__init__.py b/cython/gtsam/examples/__init__.py index ccdd2d6d2..e69de29bb 100644 --- a/cython/gtsam/examples/__init__.py +++ b/cython/gtsam/examples/__init__.py @@ -1 +0,0 @@ -from . import SFMdata diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 556e9d8d5..3b1897548 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -60,7 +60,8 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1): line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') - # # plot the covariance + # plot the covariance + # TODO (dellaert): make this work # 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 From f7c3f1da3f16e0b6fde0cb8be4e0806f8691081b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 11:30:01 -0400 Subject: [PATCH 30/39] Fixed acceleration measurement --- cython/gtsam/examples/ImuFactorExample2.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 41b06607b..9d4365b46 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -49,12 +49,8 @@ def create_poses(angular_velocity=np.pi, return poses -def ISAM2_plot(values): +def ISAM2_plot(values, fignum=0): """Plot poses.""" - - # Declare an id for the figure - fignum = 0 - fig = plt.figure(fignum) axes = fig.gca(projection='3d') plt.cla() @@ -101,6 +97,7 @@ def IMU_example(): angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step radius = 30 + acceleration = radius * angular_velocity**2 poses = create_poses(angular_velocity, delta_t, radius) # Create a factor graph @@ -171,10 +168,10 @@ def IMU_example(): totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame - # TODO: calculate correct acceleration due to circular trajectory nRb = pose_i.rotation().matrix() bRn = np.transpose(nRb) - measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + \ + vector3(0, 0, acceleration) # in body measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) @@ -194,13 +191,16 @@ def IMU_example(): isam_full.update(totalgraph, totalEstimate) result = isam_full.calculateEstimate() + ISAM2_plot(totalEstimate,0) + ISAM2_plot(result,1) + # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear() - # ISAM2_plot(result) + ISAM2_plot(result,2) if __name__ == '__main__': From d751c65fab8a8f4825f7a224c985d4df9beff0b0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 15:09:56 -0400 Subject: [PATCH 31/39] Removed batch solution (as identical) --- cython/gtsam/examples/ImuFactorExample2.py | 57 ++++++---------------- 1 file changed, 15 insertions(+), 42 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 9d4365b46..ebaac166a 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -33,7 +33,7 @@ def vector3(x, y, z): def create_poses(angular_velocity=np.pi, delta_t=0.01, radius=30.0): - # Create the set of ground-truth poses + """Create the set of ground-truth poses.""" poses = [] theta = 0.0 up = gtsam.Point3(0, 0, 1) @@ -74,22 +74,19 @@ def ISAM2_plot(values, fignum=0): plt.pause(1) -I = np.eye(3) -accCov = I * 0.1 -gyroCov = I * 0.1 -intCov = I * 0.1 -secOrder = False - # IMU preintegration parameters # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis g = 9.81 -PARAMS = gtsam.PreintegrationParams.MakeSharedU() -PARAMS.setAccelerometerCovariance(accCov) -PARAMS.setGyroscopeCovariance(gyroCov) -PARAMS.setIntegrationCovariance(intCov) -PARAMS.setUse2ndOrderCoriolis(secOrder) +I = np.eye(3) +PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) +PARAMS.setAccelerometerCovariance(I * 0.1) +PARAMS.setGyroscopeCovariance(I * 0.1) +PARAMS.setIntegrationCovariance(I * 0.1) +PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) +BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) + def IMU_example(): @@ -102,7 +99,6 @@ def IMU_example(): # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() - totalgraph = gtsam.NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = gtsam.ISAM2() @@ -110,14 +106,12 @@ def IMU_example(): # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initialEstimate = gtsam.Values() - totalEstimate = gtsam.Values() # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) - totalgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) @@ -125,18 +119,14 @@ def IMU_example(): biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) newgraph.push_back(biasprior) - totalgraph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) - totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity velocity = vector3(0, angular_velocity * radius, 0) velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) newgraph.push_back(velprior) - totalgraph.push_back(velprior) initialEstimate.insert(V(0), velocity) - totalEstimate.insert(V(0), velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) @@ -147,25 +137,17 @@ def IMU_example(): if i == 0: # First time add two poses initialEstimate.insert(X(0), poses[0].compose(delta)) initialEstimate.insert(X(1), poses[1].compose(delta)) - totalEstimate.insert(X(0), poses[0].compose(delta)) - totalEstimate.insert(X(1), poses[1].compose(delta)) elif i >= 2: # Add more poses as necessary initialEstimate.insert(X(i), pose_i.compose(delta)) - totalEstimate.insert(X(i), pose_i.compose(delta)) if i > 0: # Add Bias variables periodically if i % 5 == 0: biasKey += 1 - b1 = biasKey - 1 - b2 = biasKey - cov = gtsam.noiseModel_Isotropic.Variance(6, 0.1) - f = gtsam.BetweenFactorConstantBias( - b1, b2, gtsam.imuBias_ConstantBias(), cov) - newgraph.add(f) - totalgraph.add(f) + factor = gtsam.BetweenFactorConstantBias( + biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) + newgraph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) - totalEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = pose_i.rotation().matrix() @@ -179,29 +161,20 @@ def IMU_example(): imufac = gtsam.ImuFactor( X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) - totalgraph.add(imufac) # insert new velocity initialEstimate.insert(V(i), velocity) - totalEstimate.insert(V(i), velocity) accum.resetIntegration() - # Batch solution - isam_full = gtsam.ISAM2() - isam_full.update(totalgraph, totalEstimate) - result = isam_full.calculateEstimate() - - ISAM2_plot(totalEstimate,0) - ISAM2_plot(result,1) - # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() + ISAM2_plot(result) + + # reset newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear() - ISAM2_plot(result,2) - if __name__ == '__main__': IMU_example() From 4868a36b6cfa4f542ac6bdbb401fe270d9b40630 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 16:14:24 -0400 Subject: [PATCH 32/39] Added optional initial pose in ConstantTwistScenario --- gtsam/navigation/Scenario.h | 10 +++++++--- gtsam/navigation/tests/testScenario.cpp | 25 ++++++++++++++++++++++++- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index e4b380ff9..acb8f46f5 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,10 +57,13 @@ class Scenario { class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] - ConstantTwistScenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} + ConstantTwistScenario(const Vector3& w, const Vector3& v, + const Pose3& nTb0 = Pose3()) + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {} - Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } + Pose3 pose(double t) const override { + return nTb0_ * Pose3::Expmap(twist_ * t); + } Vector3 omega_b(double t) const override { return twist_.head<3>(); } Vector3 velocity_n(double t) const override { return rotation(t).matrix() * twist_.tail<3>(); @@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario { private: const Vector6 twist_; const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b + const Pose3 nTb0_; }; /// Accelerating from an arbitrary initial state, with optional rotation diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index bc965b058..ef30552ea 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include #include @@ -99,6 +99,29 @@ TEST(Scenario, Loop) { EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, LoopWithInitialPose) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + const Vector3 W(0, -w, 0), V(v, 0, 0); + const Rot3 nRb0 = Rot3::yaw(M_PI); + const Pose3 nTb0(nRb0, Point3(1, 2, 3)); + const ConstantTwistScenario scenario(W, V, nTb0); + + const double T = 30; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = scenario.pose(30); + EXPECT( + assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9)); +} + /* ************************************************************************* */ TEST(Scenario, Accelerating) { // Set up body pointing towards y axis, and start at 10,20,0 with velocity From 1d214d4529488b3e003b77b6529c0836c9d6128c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 17:01:47 -0400 Subject: [PATCH 33/39] Successful wrap of Scenario --- cython/gtsam/tests/test_Scenario.py | 36 +++++++++++++++++++++++++ gtsam.h | 29 +++++++++++--------- gtsam/navigation/tests/testScenario.cpp | 1 + 3 files changed, 54 insertions(+), 12 deletions(-) create mode 100644 cython/gtsam/tests/test_Scenario.py diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py new file mode 100644 index 000000000..4cca1400b --- /dev/null +++ b/cython/gtsam/tests/test_Scenario.py @@ -0,0 +1,36 @@ +import math +import unittest +import numpy as np + +import gtsam + + +class TestScenario(unittest.TestCase): + def setUp(self): + pass + + def test_loop(self): + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(6) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + scenario = gtsam.ConstantTwistScenario(W, V) + + T = 30 + np.testing.assert_almost_equal(W, scenario.omega_b(T)) + np.testing.assert_almost_equal(V, scenario.velocity_b(T)) + np.testing.assert_almost_equal( + np.cross(W, V), scenario.acceleration_b(T)) + + # R = v/w, so test if loop crests at 2*R + R = v / w + T30 = scenario.pose(T) + np.testing.assert_almost_equal( + np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) + self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + + +if __name__ == '__main__': + unittest.main() diff --git a/gtsam.h b/gtsam.h index 6e828486f..bce98b35f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2665,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ gtsam::Unit3 bRef() const; }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); void print(string s) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; @@ -2677,24 +2679,27 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ }; #include -virtual class Scenario {}; - -virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(const Vector& w, const Vector& v); +virtual class Scenario { gtsam::Pose3 pose(double t) const; Vector omega_b(double t) const; Vector velocity_n(double t) const; Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; + +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(const Vector& w, const Vector& v); + ConstantTwistScenario(const Vector& w, const Vector& v, + const Pose3& nTb0); }; 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; }; //************************************************************************* diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ef30552ea..161b8841a 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -96,6 +96,7 @@ TEST(Scenario, Loop) { const double R = v / w; const Pose3 T30 = scenario.pose(30); EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9)); + EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz())); EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } From 264a240094d62a56c3b2d364c8c08fd158e898f7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Oct 2018 19:01:28 -0400 Subject: [PATCH 34/39] Fixed both C++ and python examples --- cython/gtsam/examples/ImuFactorExample2.py | 72 +++++----- examples/ImuFactorExample2.cpp | 152 ++++++++------------- 2 files changed, 89 insertions(+), 135 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index ebaac166a..10bd3de11 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -31,24 +31,6 @@ def vector3(x, y, z): return np.array([x, y, z], dtype=np.float) -def create_poses(angular_velocity=np.pi, - delta_t=0.01, radius=30.0): - """Create the set of ground-truth poses.""" - poses = [] - theta = 0.0 - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - for i in range(80): - position = gtsam.Point3(radius * math.cos(theta), - radius * math.sin(theta), 0.0) - camera = gtsam.SimpleCamera.Lookat( - position, target, up, gtsam.Cal3_S2()) - poses.append(camera.pose()) - theta += delta_t * angular_velocity - - return poses - - def ISAM2_plot(values, fignum=0): """Plot poses.""" fig = plt.figure(fignum) @@ -77,8 +59,9 @@ def ISAM2_plot(values, fignum=0): # IMU preintegration parameters # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis g = 9.81 -I = np.eye(3) +n_gravity = vector3(0, 0, -g) PARAMS = gtsam.PreintegrationParams.MakeSharedU(g) +I = np.eye(3) PARAMS.setAccelerometerCovariance(I * 0.1) PARAMS.setGyroscopeCovariance(I * 0.1) PARAMS.setIntegrationCovariance(I * 0.1) @@ -86,16 +69,29 @@ PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) +DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), + gtsam.Point3(0.05, -0.10, 0.20)) def IMU_example(): + """Run iSAM 2 example with IMU factor.""" + + # Start with a camera on x-axis looking at origin + radius = 30 + up = gtsam.Point3(0, 0, 1) + target = gtsam.Point3(0, 0, 0) + position = gtsam.Point3(radius, 0, 0) + camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) + pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step - radius = 30 - acceleration = radius * angular_velocity**2 - poses = create_poses(angular_velocity, delta_t, radius) + + angular_velocity_vector = vector3(0, -angular_velocity, 0) + linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) + scenario = gtsam.ConstantTwistScenario( + angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() @@ -111,7 +107,7 @@ def IMU_example(): # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) - newgraph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], noise)) + newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) @@ -123,22 +119,23 @@ def IMU_example(): velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity - velocity = vector3(0, angular_velocity * radius, 0) - velprior = gtsam.PriorFactorVector(V(0), velocity, velnoise) + n_velocity = vector3(0, angular_velocity * radius, 0) + velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) - initialEstimate.insert(V(0), velocity) + initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) # Simulate poses and imu measurements, adding them to the factor graph - for i, pose_i in enumerate(poses): - delta = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), - gtsam.Point3(0.05, -0.10, 0.20)) + for i in range(80): + t = i * delta_t # simulation time if i == 0: # First time add two poses - initialEstimate.insert(X(0), poses[0].compose(delta)) - initialEstimate.insert(X(1), poses[1].compose(delta)) + pose_1 = scenario.pose(delta_t) + initialEstimate.insert(X(0), pose_0.compose(DELTA)) + initialEstimate.insert(X(1), pose_1.compose(DELTA)) elif i >= 2: # Add more poses as necessary - initialEstimate.insert(X(i), pose_i.compose(delta)) + pose_i = scenario.pose(t) + initialEstimate.insert(X(i), pose_i.compose(DELTA)) if i > 0: # Add Bias variables periodically @@ -150,11 +147,10 @@ def IMU_example(): initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame - nRb = pose_i.rotation().matrix() + nRb = scenario.rotation(t).matrix() bRn = np.transpose(nRb) - measuredAcc = - np.dot(bRn, vector3(0, 0, -g)) + \ - vector3(0, 0, acceleration) # in body - measuredOmega = np.dot(bRn, vector3(0, 0, angular_velocity)) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) + measuredOmega = scenario.omega_b(t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor @@ -162,8 +158,8 @@ def IMU_example(): X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) - # insert new velocity - initialEstimate.insert(V(i), velocity) + # insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 97002eb2a..1137ca214 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -3,42 +3,51 @@ #include #include #include +#include #include #include #include #include -#define GTSAM4 - using namespace std; using namespace gtsam; -/* ************************************************************************* */ -vector createPoses() { - // Create the set of ground-truth poses - vector poses; - double radius = 30.0, theta = 0.0; - Point3 up(0, 0, 1), target(0, 0, 0); - for (size_t i = 0; i < 80; ++i, theta += 2 * M_PI / 8) { - Point3 position(radius * cos(theta), radius * sin(theta), 0.0); - SimpleCamera camera = SimpleCamera::Lookat(position, target, up); - poses.push_back(camera.pose()); - } - return poses; -} +// Shorthand for velocity and pose variables +using symbol_shorthand::V; +using symbol_shorthand::X; + +const double kGravity = 9.81; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Shorthand for velocity and pose variables - using symbol_shorthand::V; - using symbol_shorthand::X; + auto params = PreintegrationParams::MakeSharedU(kGravity); + params->setAccelerometerCovariance(I_3x3 * 0.1); + params->setGyroscopeCovariance(I_3x3 * 0.1); + params->setIntegrationCovariance(I_3x3 * 0.1); + params->setUse2ndOrderCoriolis(false); + params->setOmegaCoriolis(Vector3(0, 0, 0)); - // Create the set of ground-truth landmarks and poses - vector poses = createPoses(); + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + + // Start with a camera on x-axis looking at origin + double radius = 30; + const Point3 up(0, 0, 1), target(0, 0, 0); + const Point3 position(radius, 0, 0); + const SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + const auto pose_0 = camera.pose(); + + // Now, create a constant-twist scenario that makes the camera orbit the + // origin + double angular_velocity = M_PI, // rad/sec + delta_t = 1.0 / 18; // makes for 10 degrees per step + Vector3 angular_velocity_vector(0, -angular_velocity, 0); + Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0); + auto scenario = ConstantTwistScenario(angular_velocity_vector, + linear_velocity_vector, pose_0); // Create a factor graph - NonlinearFactorGraph newgraph, totalgraph; + NonlinearFactorGraph newgraph; // Create (incremental) ISAM2 solver ISAM2 isam; @@ -51,8 +60,7 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - newgraph.push_back(PriorFactor(X(0), poses[0], noise)); - totalgraph.push_back(PriorFactor(X(0), poses[0], noise)); + newgraph.push_back(PriorFactor(X(0), pose_0, noise)); // Add imu priors Key biasKey = Symbol('b', 0); @@ -60,65 +68,29 @@ int main(int argc, char* argv[]) { PriorFactor biasprior(biasKey, imuBias::ConstantBias(), biasnoise); newgraph.push_back(biasprior); - totalgraph.push_back(biasprior); initialEstimate.insert(biasKey, imuBias::ConstantBias()); - totalEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); - Vector gravity(3), zero(3), velocity(3); - gravity << 0, 0, -9.8; - zero << 0, 0, 0; - velocity << 0, 0, 0; -#ifdef GTSAM4 - PriorFactor velprior(V(0), zero, velnoise); -#else - PriorFactor velprior(V(0), zero, velnoise); -#endif + Vector n_velocity(3); + n_velocity << 0, angular_velocity * radius, 0; + PriorFactor velprior(V(0), n_velocity, velnoise); newgraph.push_back(velprior); - totalgraph.push_back(velprior); -#ifdef GTSAM4 - initialEstimate.insert(V(0), zero); - totalEstimate.insert(V(0), zero); -#else - initialEstimate.insert(V(0), LieVector(zero)); - totalEstimate.insert(V(0), LieVector(zero)); -#endif + initialEstimate.insert(V(0), n_velocity); - Matrix3 I; - I << 1, 0, 0, 0, 1, 0, 0, 0, 1; - Matrix3 accCov = I * 0.1; - Matrix3 gyroCov = I * 0.1; - Matrix3 intCov = I * 0.1; - bool secOrder = false; -#ifdef GTSAM4 // IMU preintegrator - PreintegratedImuMeasurements accum(PreintegrationParams::MakeSharedD()); - accum.params()->setAccelerometerCovariance(accCov); - accum.params()->setGyroscopeCovariance(gyroCov); - accum.params()->setIntegrationCovariance(intCov); - accum.params()->setUse2ndOrderCoriolis(secOrder); - accum.params()->setOmegaCoriolis(Vector3(0, 0, 0)); -#else - ImuFactor::PreintegratedMeasurements accum(imuBias::ConstantBias(), accCov, - gyroCov, intCov, secOrder); -#endif + PreintegratedImuMeasurements accum(params); // Simulate poses and imu measurements, adding them to the factor graph - for (size_t i = 0; i < poses.size(); ++i) { -#ifdef GTSAM4 - Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); -#else - Pose3 delta(Rot3::ypr(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); -#endif + for (size_t i = 0; i < 36; ++i) { + double t = i * delta_t; if (i == 0) { // First time add two poses - initialEstimate.insert(X(0), poses[0].compose(delta)); - initialEstimate.insert(X(1), poses[1].compose(delta)); - totalEstimate.insert(X(0), poses[0].compose(delta)); - totalEstimate.insert(X(1), poses[1].compose(delta)); + auto pose_1 = scenario.pose(delta_t); + initialEstimate.insert(X(0), pose_0.compose(delta)); + initialEstimate.insert(X(1), pose_1.compose(delta)); } else if (i >= 2) { // Add more poses as necessary - initialEstimate.insert(X(i), poses[i].compose(delta)); - totalEstimate.insert(X(i), poses[i].compose(delta)); + auto pose_i = scenario.pose(t); + initialEstimate.insert(X(i), pose_i.compose(delta)); } if (i > 0) { @@ -135,44 +107,30 @@ int main(int argc, char* argv[]) { auto f = boost::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); - totalgraph.add(f); initialEstimate.insert(biasKey, imuBias::ConstantBias()); - totalEstimate.insert(biasKey, imuBias::ConstantBias()); } - // Add Imu Factor - accum.integrateMeasurement(gravity, zero, 0.5); -#ifdef GTSAM4 - ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); -#else - ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum, gravity, - zero); -#endif - newgraph.add(imufac); - totalgraph.add(imufac); + // Predict acceleration and gyro measurements in (actual) body frame + auto measuredAcc = scenario.acceleration_b(t) - + scenario.rotation(t).transpose() * params->n_gravity; + auto measuredOmega = scenario.omega_b(t); + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t); - // insert new velocity -#ifdef GTSAM4 - initialEstimate.insert(V(i), velocity); - totalEstimate.insert(V(i), velocity); -#else - initialEstimate.insert(V(i), LieVector(velocity)); - totalEstimate.insert(V(i), LieVector(velocity)); -#endif + // Add Imu Factor + ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum); + newgraph.add(imufac); + + // insert new velocity, which is wrong + initialEstimate.insert(V(i), n_velocity); accum.resetIntegration(); } - // Batch solution - ISAM2 isam_full; - isam_full.update(totalgraph, totalEstimate); - result = isam_full.calculateEstimate(); - // Incremental solution isam.update(newgraph, initialEstimate); result = isam.calculateEstimate(); newgraph = NonlinearFactorGraph(); initialEstimate.clear(); } - + GTSAM_PRINT(result); return 0; } /* ************************************************************************* */ From d802255eb3fb4302b408c6f8422d5cc4764d3565 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Tue, 16 Oct 2018 07:06:39 -0400 Subject: [PATCH 35/39] support pythonic print for gtsam objects. --- cython/gtsam/examples/OdometryExample.py | 6 +++--- cython/gtsam/examples/PlanarSLAMExample.py | 6 +++--- cython/gtsam/examples/Pose2SLAMExample.py | 6 +++--- cython/gtsam/examples/README.md | 4 ++-- wrap/Method.cpp | 2 +- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py index 0357aebb3..5d2190d56 100644 --- a/cython/gtsam/examples/OdometryExample.py +++ b/cython/gtsam/examples/OdometryExample.py @@ -35,7 +35,7 @@ odometry = gtsam.Pose2(2.0, 0.0, 0.0) # Create odometry (Between) factors between consecutive poses graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) -graph.print_("\nFactor Graph:\n") +print("\nFactor Graph:\n{}".format(graph)) # Create the data structure to hold the initialEstimate estimate to the solution # For illustrative purposes, these have been deliberately set to incorrect values @@ -43,10 +43,10 @@ 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") +print("\nInitial Estimate:\n{}".format(initial)) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() -result.print_("\nFinal Result:\n") +print("\nFinal Result:\n{}".format(result)) diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py index 785faaea6..fb038d043 100644 --- a/cython/gtsam/examples/PlanarSLAMExample.py +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -50,7 +50,7 @@ graph.add(gtsam.BearingRangeFactor2D( X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) # Print graph -graph.print_("Factor Graph:\n") +print("Factor Graph:\n{}".format(graph)) # Create (deliberately inaccurate) initial estimate initial_estimate = gtsam.Values() @@ -61,7 +61,7 @@ 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") +print("Initial Estimate:\n{}".format(initial_estimate)) # Optimize using Levenberg-Marquardt optimization. The optimizer # accepts an optional set of configuration parameters, controlling @@ -72,7 +72,7 @@ initial_estimate.print_("Initial Estimate:\n") params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) result = optimizer.optimize() -result.print_("\nFinal Result:\n") +print("\nFinal Result:\n{}".format(result)) # Calculate and print marginal covariances for all variables marginals = gtsam.Marginals(graph, result) diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index a9bb9d74b..b15b90864 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -52,7 +52,7 @@ graph.add(gtsam.BetweenFactorPose2( # 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), ODOMETRY_NOISE)) -graph.print_("\nFactor Graph:\n") # print +print("\nFactor Graph:\n{}".format(graph)) # 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 @@ -62,7 +62,7 @@ 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 +print("\nInitial Estimate:\n{}".format(initial_estimate)) # print # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer # The optimizer accepts an optional set of configuration parameters, @@ -79,7 +79,7 @@ parameters.setMaxIterations(100) optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) # ... and optimize result = optimizer.optimize() -result.print_("Final Result:\n") +print("Final Result:\n{}".format(result)) # 5. Calculate and print marginal covariances for all variables marginals = gtsam.Marginals(graph, result) diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 4474da488..067929a20 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -1,4 +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... +examples. However, there are just some slight name changes, for example +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/wrap/Method.cpp b/wrap/Method.cpp index 2637275d1..1a58692b6 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -106,7 +106,7 @@ void Method::emit_cython_pyx_no_overload(FileWriter& file, // leverage python's special treatment for print if (funcName == "print_") { - file.oss << " def __str__(self):\n"; + file.oss << " def __repr__(self):\n"; file.oss << " strBuf = RedirectCout()\n"; file.oss << " self.print_('')\n"; file.oss << " return strBuf.str()\n"; From 15cc9a51b3b66eb1a6d3f3b201000141bed37b3e Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Wed, 17 Oct 2018 06:01:14 -0400 Subject: [PATCH 36/39] fix "const Vector&" in gtsam.h --- gtsam.h | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam.h b/gtsam.h index bce98b35f..ca37d2d62 100644 --- a/gtsam.h +++ b/gtsam.h @@ -568,7 +568,7 @@ class Pose2 { static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; + Vector Adjoint(Vector xi) const; static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 @@ -865,7 +865,7 @@ class CalibratedCamera { // Standard Constructors and Named Constructors CalibratedCamera(); CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); + CalibratedCamera(Vector v); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable @@ -875,7 +875,7 @@ class CalibratedCamera { // Manifold static size_t Dim(); size_t dim() const; - gtsam::CalibratedCamera retract(const Vector& d) const; + gtsam::CalibratedCamera retract(Vector d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; // Action on Point3 @@ -911,7 +911,7 @@ class PinholeCamera { CALIBRATION calibration() const; // Manifold - This retract(const Vector& d) const; + This retract(Vector d) const; Vector localCoordinates(const This& T2) const; size_t dim() const; static size_t Dim(); @@ -950,7 +950,7 @@ virtual class SimpleCamera { gtsam::Cal3_S2 calibration() const; // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; + gtsam::SimpleCamera retract(Vector d) const; Vector localCoordinates(const gtsam::SimpleCamera& T2) const; size_t dim() const; static size_t Dim(); @@ -992,7 +992,7 @@ class StereoCamera { gtsam::Cal3_S2Stereo calibration() const; // Manifold - gtsam::StereoCamera retract(const Vector& d) const; + gtsam::StereoCamera retract(Vector d) const; Vector localCoordinates(const gtsam::StereoCamera& T2) const; size_t dim() const; static size_t Dim(); @@ -1227,12 +1227,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { }; virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); static gtsam::noiseModel::Constrained* All(size_t dim); static gtsam::noiseModel::Constrained* All(size_t dim, double mu); @@ -1415,12 +1415,12 @@ virtual class JacobianFactor : gtsam::GaussianFactor { pair jacobianUnweighted() const; Matrix augmentedJacobianUnweighted() const; - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; pair eliminate(const gtsam::Ordering& keys) const; - void setModel(bool anyConstrained, const Vector& sigmas); + void setModel(bool anyConstrained, Vector sigmas); gtsam::noiseModel::Diagonal* get_model() const; @@ -2691,15 +2691,15 @@ virtual class Scenario { }; virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(const Vector& w, const Vector& v); - ConstantTwistScenario(const Vector& w, const Vector& v, + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, const Pose3& nTb0); }; 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); + Vector v0, Vector a_n, + Vector omega_b); }; //************************************************************************* From f4564f444e75bf7405ec666733200379aaff6b61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Oct 2018 10:47:03 -0400 Subject: [PATCH 37/39] 2D pose plotting --- cython/gtsam/utils/plot.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 3b1897548..19402a080 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -4,6 +4,31 @@ import numpy as np import matplotlib.pyplot as plt +def plot_pose2_on_axes(axes, pose, axis_length=0.1): + """Plot a 2D 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()]) + + # 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], '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], 'g-') + + +def plot_pose2(fignum, pose, axis_length=0.1): + """Plot a 2D pose on given figure with given 'axis_length'.""" + # get figure object + fig = plt.figure(fignum) + axes = fig.gca() + plot_pose2_on_axes(axes, pose, axis_length) + + 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) From 984028697979e37c7ed9b05c09f37334f442a827 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 22 Oct 2018 11:31:12 +0000 Subject: [PATCH 38/39] Fixed noise model, removed extraneous variable --- examples/ImuFactorExample2.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 1137ca214..b4ad5d574 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -57,9 +57,9 @@ int main(int argc, char* argv[]) { Values initialEstimate, totalEstimate, result; // Add a prior on pose x0. This indirectly specifies where the origin is. - // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z. auto noise = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); newgraph.push_back(PriorFactor(X(0), pose_0, noise)); // Add imu priors @@ -102,8 +102,6 @@ int main(int argc, char* argv[]) { Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; auto cov = noiseModel::Diagonal::Variances(covvec); - Vector6 zerovec; - zerovec << 0, 0, 0, 0, 0, 0; auto f = boost::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); From 2a0cee830d7278bb79aafc330f838f6c934e3878 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 23 Oct 2018 19:55:41 +0000 Subject: [PATCH 39/39] Updated outdated comment --- gtsam/inference/Symbol.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 2d7ede8e7..6b5bfcc03 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -26,10 +26,9 @@ namespace gtsam { /** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. + * Character and index key used to refer to variables. Will simply cast to a Key, + * i.e., a large integer. Keys are used to retrieve values from Values, + * specify what variables factors depend on, etc. */ class GTSAM_EXPORT Symbol { protected: