From 025450dcc5a44996a91e157e9b8db2e6efda7adb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 14 Oct 2018 08:06:59 -0400 Subject: [PATCH] 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