diff --git a/python/gtsam_py/python/__init__.py b/python/gtsam_py/python/__init__.py new file mode 100644 index 000000000..d40ee4502 --- /dev/null +++ b/python/gtsam_py/python/__init__.py @@ -0,0 +1,26 @@ +from .gtsam import * + +try: + import gtsam_unstable + + + def _deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + + 'Please import it from gtsam_unstable.') + warn(message) + return item(*args, **kwargs) + return wrapper + + + for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = _deprecated_wrapper(item, name) + globals()[name] = item + +except ImportError: + pass + diff --git a/python/gtsam_py/python/examples/DogLegOptimizerExample.py b/python/gtsam_py/python/examples/DogLegOptimizerExample.py new file mode 100644 index 000000000..776ceedc4 --- /dev/null +++ b/python/gtsam_py/python/examples/DogLegOptimizerExample.py @@ -0,0 +1,118 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Example comparing DoglegOptimizer with Levenberg-Marquardt. +Author: Frank Dellaert +""" +# pylint: disable=no-member, invalid-name + +import math +import argparse + +import gtsam +import matplotlib.pyplot as plt +import numpy as np + + +def run(args): + """Test Dogleg vs LM, inspired by issue #452.""" + + # print parameters + print("num samples = {}, deltaInitial = {}".format( + args.num_samples, args.delta)) + + # Ground truth solution + T11 = gtsam.Pose2(0, 0, 0) + T12 = gtsam.Pose2(1, 0, 0) + T21 = gtsam.Pose2(0, 1, 0) + T22 = gtsam.Pose2(1, 1, 0) + + # Factor graph + graph = gtsam.NonlinearFactorGraph() + + # Priors + prior = gtsam.noiseModel_Isotropic.Sigma(3, 1) + graph.add(gtsam.PriorFactorPose2(11, T11, prior)) + graph.add(gtsam.PriorFactorPose2(21, T21, prior)) + + # Odometry + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) + graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) + graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) + + # Range + model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01) + graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) + + params = gtsam.DoglegParams() + params.setDeltaInitial(args.delta) # default is 10 + + # Add progressively more noise to ground truth + sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] + n = len(sigmas) + p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n + for i, sigma in enumerate(sigmas): + dl_fails, lm_fails = 0, 0 + # Attempt num_samples optimizations for both DL and LM + for _attempt in range(args.num_samples): + initial = gtsam.Values() + initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) + initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) + initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) + initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) + + # Run dogleg optimizer + dl = gtsam.DoglegOptimizer(graph, initial, params) + result = dl.optimize() + dl_fails += graph.error(result) > 1e-9 + + # Run + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) + result = lm.optimize() + lm_fails += graph.error(result) > 1e-9 + + # Calculate Bayes estimate of success probability + # using a beta prior of alpha=0.5, beta=0.5 + alpha, beta = 0.5, 0.5 + v = args.num_samples+alpha+beta + p_dl[i] = (args.num_samples-dl_fails+alpha)/v + p_lm[i] = (args.num_samples-lm_fails+alpha)/v + + def stddev(p): + """Calculate standard deviation.""" + return math.sqrt(p*(1-p)/(1+v)) + + s_dl[i] = stddev(p_dl[i]) + s_lm[i] = stddev(p_lm[i]) + + fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" + print(fmt.format(sigma, + 100*p_dl[i], 100*s_dl[i], + 100*p_lm[i], 100*s_lm[i])) + + if args.plot: + fig, ax = plt.subplots() + dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") + lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") + plt.title("Dogleg emprical success vs. LM") + plt.legend(handles=[dl_plot, lm_plot]) + ax.set_xlim(0, sigmas[-1]+1) + ax.set_ylim(0, 1) + plt.show() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Compare Dogleg and LM success rates") + parser.add_argument("-n", "--num_samples", type=int, default=1000, + help="Number of samples for each sigma") + parser.add_argument("-d", "--delta", type=float, default=10.0, + help="Initial delta for dogleg") + parser.add_argument("-p", "--plot", action="store_true", + help="Flag to plot results") + args = parser.parse_args() + run(args) diff --git a/python/gtsam_py/python/examples/GPSFactorExample.py b/python/gtsam_py/python/examples/GPSFactorExample.py new file mode 100644 index 000000000..493a07725 --- /dev/null +++ b/python/gtsam_py/python/examples/GPSFactorExample.py @@ -0,0 +1,56 @@ +""" +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) + +See LICENSE for the license information + +Simple robot motion example, with prior and one GPS measurements +Author: Mandy Xie +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + +# ENU Origin is where the plane was in hold next to runway +lat0 = 33.86998 +lon0 = -84.30626 +h0 = 274 + +# Create noise models +GPS_NOISE = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) +PRIOR_NOISE = gtsam.noiseModel_Isotropic.Sigma(6, 0.25) + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first point, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose3() # prior at origin +graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) + +# Add GPS factors +gps = gtsam.Point3(lat0, lon0, h0) +graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) +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 +initial = gtsam.Values() +initial.insert(1, gtsam.Pose3()) +print("\nInitial Estimate:\n{}".format(initial)) + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +print("\nFinal Result:\n{}".format(result)) + diff --git a/python/gtsam_py/python/examples/ImuFactorExample.py b/python/gtsam_py/python/examples/ImuFactorExample.py new file mode 100644 index 000000000..0e01766e7 --- /dev/null +++ b/python/gtsam_py/python/examples/ImuFactorExample.py @@ -0,0 +1,153 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +A script validating and demonstrating the ImuFactor inference. + +Author: Frank Dellaert, Varun Agrawal +""" + +from __future__ import print_function + +import math + +import gtsam +import matplotlib.pyplot as plt +import numpy as np +from gtsam import symbol_shorthand_B as B +from gtsam import symbol_shorthand_V as V +from gtsam import symbol_shorthand_X as X +from gtsam.utils.plot import plot_pose3 +from mpl_toolkits.mplot3d import Axes3D + +from PreintegrationExample import POSES_FIG, PreintegrationExample + +BIAS_KEY = B(0) + + +np.set_printoptions(precision=3, suppress=True) + + +class ImuFactorExample(PreintegrationExample): + + def __init__(self): + self.velocity = np.array([2, 0, 0]) + self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + + # Choose one of these twists to change scenario: + zero_twist = (np.zeros(3), np.zeros(3)) + forward_twist = (np.zeros(3), self.velocity) + loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) + sick_twist = ( + np.array([math.radians(30), -math.radians(30), 0]), self.velocity) + + accBias = np.array([-0.3, 0.1, 0.2]) + gyroBias = np.array([0.1, 0.3, -0.1]) + bias = gtsam.imuBias_ConstantBias(accBias, gyroBias) + + dt = 1e-2 + super(ImuFactorExample, self).__init__(sick_twist, bias, dt) + + def addPrior(self, i, graph): + state = self.scenario.navState(i) + graph.push_back(gtsam.PriorFactorPose3( + X(i), state.pose(), self.priorNoise)) + graph.push_back(gtsam.PriorFactorVector( + V(i), state.velocity(), self.velNoise)) + + def run(self): + graph = gtsam.NonlinearFactorGraph() + + # initialize data structure for pre-integrated IMU measurements + pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) + + T = 12 + num_poses = T + 1 # assumes 1 factor per second + initial = gtsam.Values() + initial.insert(BIAS_KEY, self.actualBias) + for i in range(num_poses): + state_i = self.scenario.navState(float(i)) + + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + pose = state_i.pose().compose(poseNoise) + + velocity = state_i.velocity() + np.random.randn(3)*0.1 + + initial.insert(X(i), pose) + initial.insert(V(i), velocity) + + # simulate the loop + i = 0 # state index + actual_state_i = self.scenario.navState(0) + for k, t in enumerate(np.arange(0, T, self.dt)): + # get measurements and add them to PIM + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) + + poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1) + + actual_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3)*0.1) + + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + + # Plot every second + if k % int(1 / self.dt) == 0: + self.plotGroundTruthPose(t) + + # create IMU factor every second + if (k + 1) % int(1 / self.dt) == 0: + factor = gtsam.ImuFactor(X(i), V(i), X( + i + 1), V(i + 1), BIAS_KEY, pim) + graph.push_back(factor) + if True: + print(factor) + print(pim.predict(actual_state_i, self.actualBias)) + pim.resetIntegration() + actual_state_i = self.scenario.navState(t + self.dt) + i += 1 + + # add priors on beginning and end + self.addPrior(0, graph) + self.addPrior(num_poses - 1, graph) + + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("SUMMARY") + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + + # Calculate and print marginal covariances + marginals = gtsam.Marginals(graph, result) + print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) + for i in range(num_poses): + print("Covariance on pose {}:\n{}\n".format( + i, marginals.marginalCovariance(X(i)))) + print("Covariance on vel {}:\n{}\n".format( + i, marginals.marginalCovariance(V(i)))) + + # Plot resulting poses + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + plot_pose3(POSES_FIG, pose_i, 0.1) + i += 1 + + gtsam.utils.plot.set_axes_equal(POSES_FIG) + + print(result.atimuBias_ConstantBias(BIAS_KEY)) + + plt.ioff() + plt.show() + + +if __name__ == '__main__': + ImuFactorExample().run() diff --git a/python/gtsam_py/python/examples/ImuFactorExample2.py b/python/gtsam_py/python/examples/ImuFactorExample2.py new file mode 100644 index 000000000..0d98f08fe --- /dev/null +++ b/python/gtsam_py/python/examples/ImuFactorExample2.py @@ -0,0 +1,172 @@ +""" +iSAM2 example with ImuFactor. +Author: Robert Truax (C++), Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import math + +import gtsam +import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np +from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, + ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, + PinholeCameraCal3_S2, Point3, Pose3, + PriorFactorConstantBias, PriorFactorPose3, + PriorFactorVector, Rot3, Values) +from gtsam import symbol_shorthand_B as B +from gtsam import symbol_shorthand_V as V +from gtsam import symbol_shorthand_X as X +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 + + +def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=np.float) + + +def ISAM2_plot(values, fignum=0): + """Plot poses.""" + 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) + + +# IMU preintegration parameters +# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis +g = 9.81 +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) +PARAMS.setUse2ndOrderCoriolis(False) +PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) + +BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) +DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), + 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 = Point3(0, 0, 1) + target = Point3(0, 0, 0) + position = Point3(radius, 0, 0) + camera = PinholeCameraCal3_S2.Lookat(position, target, up, 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 + + angular_velocity_vector = vector3(0, -angular_velocity, 0) + linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) + scenario = ConstantTwistScenario( + angular_velocity_vector, linear_velocity_vector, pose_0) + + # Create a factor graph + newgraph = NonlinearFactorGraph() + + # Create (incremental) ISAM2 solver + isam = ISAM2() + + # Create the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initialEstimate = 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.1, 0.1, 0.1, 0.3, 0.3, 0.3])) + newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) + + # Add imu priors + biasKey = B(0) + biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), + biasnoise) + newgraph.push_back(biasprior) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + + # Calculate with correct initial velocity + n_velocity = vector3(0, angular_velocity * radius, 0) + velprior = PriorFactorVector(V(0), n_velocity, velnoise) + newgraph.push_back(velprior) + initialEstimate.insert(V(0), n_velocity) + + accum = gtsam.PreintegratedImuMeasurements(PARAMS) + + # Simulate poses and imu measurements, adding them to the factor graph + for i in range(80): + t = i * delta_t # simulation time + if i == 0: # First time add two poses + 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 + pose_i = scenario.pose(t) + initialEstimate.insert(X(i), pose_i.compose(DELTA)) + + if i > 0: + # Add Bias variables periodically + if i % 5 == 0: + biasKey += 1 + factor = BetweenFactorConstantBias( + biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) + newgraph.add(factor) + initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) + + # Predict acceleration and gyro measurements in (actual) body frame + nRb = scenario.rotation(t).matrix() + bRn = np.transpose(nRb) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) + measuredOmega = scenario.omega_b(t) + accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) + + # Add Imu Factor + imufac = ImuFactor(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() + + # Incremental solution + isam.update(newgraph, initialEstimate) + result = isam.calculateEstimate() + ISAM2_plot(result) + + # reset + newgraph = NonlinearFactorGraph() + initialEstimate.clear() + + +if __name__ == '__main__': + IMU_example() diff --git a/python/gtsam_py/python/examples/OdometryExample.py b/python/gtsam_py/python/examples/OdometryExample.py new file mode 100644 index 000000000..e778e3f85 --- /dev/null +++ b/python/gtsam_py/python/examples/OdometryExample.py @@ -0,0 +1,69 @@ +""" +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) + +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 + +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + +# 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 +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 +# 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)) +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 +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)) +print("\nInitial Estimate:\n{}".format(initial)) + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +print("\nFinal Result:\n{}".format(result)) + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +for i in range(1, 4): + print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + +fig = plt.figure(0) +for i in range(1, 4): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) +plt.axis('equal') +plt.show() + + + diff --git a/python/gtsam_py/python/examples/PlanarManipulatorExample.py b/python/gtsam_py/python/examples/PlanarManipulatorExample.py new file mode 100644 index 000000000..e42ae09d7 --- /dev/null +++ b/python/gtsam_py/python/examples/PlanarManipulatorExample.py @@ -0,0 +1,334 @@ +""" +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) + +See LICENSE for the license information + +Kinematics of three-link manipulator with GTSAM poses and product of exponential maps. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import math +import unittest +from functools import reduce + +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 import Pose2 +from gtsam.utils.test_case import GtsamTestCase + + +def vector3(x, y, z): + """Create 3D double numpy array.""" + return np.array([x, y, z], dtype=np.float) + + +def compose(*poses): + """Compose all Pose2 transforms given as arguments from left to right.""" + return reduce((lambda x, y: x.compose(y)), poses) + + +def vee(M): + """Pose2 vee operator.""" + return vector3(M[0, 2], M[1, 2], M[1, 0]) + + +def delta(g0, g1): + """Difference between x,y,,theta components of SE(2) poses.""" + return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta()) + + +def trajectory(g0, g1, N=20): + """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately. + g0 and g1 are the initial and final pose, respectively. + N is the number of *intervals* + Returns N+1 poses + """ + e = delta(g0, g1) + return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)] + + +class ThreeLinkArm(object): + """Three-link arm class.""" + + def __init__(self): + self.L1 = 3.5 + self.L2 = 3.5 + self.L3 = 2.5 + self.xi1 = vector3(0, 0, 1) + self.xi2 = vector3(self.L1, 0, 1) + self.xi3 = vector3(self.L1+self.L2, 0, 1) + self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90)) + + def fk(self, q): + """ Forward kinematics. + Takes numpy array of joint angles, in radians. + """ + sXl1 = Pose2(0, 0, math.radians(90)) + l1Zl1 = Pose2(0, 0, q[0]) + l1Xl2 = Pose2(self.L1, 0, 0) + l2Zl2 = Pose2(0, 0, q[1]) + l2Xl3 = Pose2(self.L2, 0, 0) + l3Zl3 = Pose2(0, 0, q[2]) + l3Xt = Pose2(self.L3, 0, 0) + return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt) + + def jacobian(self, q): + """ Calculate manipulator Jacobian. + Takes numpy array of joint angles, in radians. + """ + a = q[0]+q[1] + b = a+q[2] + return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b), + -self.L1*math.cos(a)-self.L3*math.cos(b), + - self.L3*math.cos(b)], + [-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b), + -self.L1*math.sin(a)-self.L3*math.sin(b), + - self.L3*math.sin(b)], + [1, 1, 1]], np.float) + + def poe(self, q): + """ Forward kinematics. + Takes numpy array of joint angles, in radians. + """ + l1Zl1 = Pose2.Expmap(self.xi1 * q[0]) + l2Zl2 = Pose2.Expmap(self.xi2 * q[1]) + l3Zl3 = Pose2.Expmap(self.xi3 * q[2]) + return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) + + def con(self, q): + """ Forward kinematics, conjugation form. + Takes numpy array of joint angles, in radians. + """ + def expmap(x, y, theta): + """Implement exponential map via conjugation with axis (x,y).""" + return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0)) + + l1Zl1 = expmap(0.0, 0.0, q[0]) + l2Zl2 = expmap(0.0, self.L1, q[1]) + l3Zl3 = expmap(0.0, self.L1+self.L2, q[2]) + return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) + + def ik(self, sTt_desired, e=1e-9): + """ Inverse kinematics. + Takes desired Pose2 of tool T with respect to base S. + Optional: mu, gradient descent rate; e: error norm threshold + """ + q = np.radians(vector3(30, -30, 45)) # well within workspace + error = vector3(100, 100, 100) + + while np.linalg.norm(error) > e: + error = delta(sTt_desired, self.fk(q)) + J = self.jacobian(q) + q -= np.dot(np.linalg.pinv(J), error) + + # return result in interval [-pi,pi) + return np.remainder(q+math.pi, 2*math.pi)-math.pi + + def manipulator_jacobian(self, q): + """ Calculate manipulator Jacobian. + Takes numpy array of joint angles, in radians. + Returns the manipulator Jacobian of differential twists. When multiplied with + a vector of joint velocities, will yield a single differential twist which is + the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose. + Just like always, differential twists can be hatted and multiplied with spatial + coordinates of a point to give the spatial velocity of the point. + """ + l1Zl1 = Pose2.Expmap(self.xi1 * q[0]) + l2Zl2 = Pose2.Expmap(self.xi2 * q[1]) + # l3Zl3 = Pose2.Expmap(self.xi3 * q[2]) + + p1 = self.xi1 + # p1 = Pose2().Adjoint(self.xi1) + + sTl1 = l1Zl1 + p2 = sTl1.Adjoint(self.xi2) + + sTl2 = compose(l1Zl1, l2Zl2) + p3 = sTl2.Adjoint(self.xi3) + + differential_twists = [p1, p2, p3] + return np.stack(differential_twists, axis=1) + + def plot(self, fignum, q): + """ Plot arm. + Takes figure number, and numpy array of joint angles, in radians. + """ + fig = plt.figure(fignum) + axes = fig.gca() + + sXl1 = Pose2(0, 0, math.radians(90)) + t = sXl1.translation() + p1 = np.array([t.x(), t.y()]) + gtsam_plot.plot_pose2_on_axes(axes, sXl1) + + def plot_line(p, g, color): + t = g.translation() + q = np.array([t.x(), t.y()]) + line = np.append(p[np.newaxis], q[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], color) + return q + + l1Zl1 = Pose2(0, 0, q[0]) + l1Xl2 = Pose2(self.L1, 0, 0) + sTl2 = compose(sXl1, l1Zl1, l1Xl2) + p2 = plot_line(p1, sTl2, 'r-') + gtsam_plot.plot_pose2_on_axes(axes, sTl2) + + l2Zl2 = Pose2(0, 0, q[1]) + l2Xl3 = Pose2(self.L2, 0, 0) + sTl3 = compose(sTl2, l2Zl2, l2Xl3) + p3 = plot_line(p2, sTl3, 'g-') + gtsam_plot.plot_pose2_on_axes(axes, sTl3) + + l3Zl3 = Pose2(0, 0, q[2]) + l3Xt = Pose2(self.L3, 0, 0) + sTt = compose(sTl3, l3Zl3, l3Xt) + plot_line(p3, sTt, 'b-') + gtsam_plot.plot_pose2_on_axes(axes, sTt) + + +# Create common example configurations. +Q0 = vector3(0, 0, 0) +Q1 = np.radians(vector3(-30, -45, -90)) +Q2 = np.radians(vector3(-90, 90, 0)) + + +class TestPose2SLAMExample(GtsamTestCase): + """Unit tests for functions used below.""" + + def setUp(self): + self.arm = ThreeLinkArm() + + def assertPose2Equals(self, actual, expected, tol=1e-2): + """Helper function that prints out actual and expected if not equal.""" + equal = actual.equals(expected, tol) + if not equal: + raise self.failureException( + "Poses are not equal:\n{}!={}".format(actual, expected)) + + def test_fk_arm(self): + """Make sure forward kinematics is correct for some known test configurations.""" + # at rest + expected = Pose2(0, 2*3.5 + 2.5, math.radians(90)) + sTt = self.arm.fk(Q0) + self.assertIsInstance(sTt, Pose2) + self.assertPose2Equals(sTt, expected) + + # -30, -45, -90 + expected = Pose2(5.78, 1.52, math.radians(-75)) + sTt = self.arm.fk(Q1) + self.assertPose2Equals(sTt, expected) + + def test_jacobian(self): + """Test Jacobian calculation.""" + # at rest + expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float) + J = self.arm.jacobian(Q0) + np.testing.assert_array_almost_equal(J, expected) + + # at -90, 90, 0 + expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float) + J = self.arm.jacobian(Q2) + np.testing.assert_array_almost_equal(J, expected) + + def test_con_arm(self): + """Make sure POE is correct for some known test configurations.""" + # at rest + expected = Pose2(0, 2*3.5 + 2.5, math.radians(90)) + sTt = self.arm.con(Q0) + self.assertIsInstance(sTt, Pose2) + self.assertPose2Equals(sTt, expected) + + # -30, -45, -90 + expected = Pose2(5.78, 1.52, math.radians(-75)) + sTt = self.arm.con(Q1) + self.assertPose2Equals(sTt, expected) + + def test_poe_arm(self): + """Make sure POE is correct for some known test configurations.""" + # at rest + expected = Pose2(0, 2*3.5 + 2.5, math.radians(90)) + sTt = self.arm.poe(Q0) + self.assertIsInstance(sTt, Pose2) + self.assertPose2Equals(sTt, expected) + + # -30, -45, -90 + expected = Pose2(5.78, 1.52, math.radians(-75)) + sTt = self.arm.poe(Q1) + self.assertPose2Equals(sTt, expected) + + def test_ik(self): + """Check iterative inverse kinematics function.""" + # at rest + actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90))) + np.testing.assert_array_almost_equal(actual, Q0, decimal=2) + + # -30, -45, -90 + sTt_desired = Pose2(5.78, 1.52, math.radians(-75)) + actual = self.arm.ik(sTt_desired) + self.assertPose2Equals(self.arm.fk(actual), sTt_desired) + np.testing.assert_array_almost_equal(actual, Q1, decimal=2) + + def test_manipulator_jacobian(self): + """Test Jacobian calculation.""" + # at rest + expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float) + J = self.arm.manipulator_jacobian(Q0) + np.testing.assert_array_almost_equal(J, expected) + + # at -90, 90, 0 + expected = np.array( + [[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float) + J = self.arm.manipulator_jacobian(Q2) + np.testing.assert_array_almost_equal(J, expected) + + +def run_example(): + """ Use trajectory interpolation and then trajectory tracking a la Murray + to move a 3-link arm on a straight line. + """ + # Create arm + arm = ThreeLinkArm() + + # Get initial pose using forward kinematics + q = np.radians(vector3(30, -30, 45)) + sTt_initial = arm.fk(q) + + # Create interpolated trajectory in task space to desired goal pose + sTt_goal = Pose2(2.4, 4.3, math.radians(0)) + poses = trajectory(sTt_initial, sTt_goal, 50) + + # Setup figure and plot initial pose + fignum = 0 + fig = plt.figure(fignum) + axes = fig.gca() + axes.set_xlim(-5, 5) + axes.set_ylim(0, 10) + gtsam_plot.plot_pose2(fignum, arm.fk(q)) + + # For all poses in interpolated trajectory, calculate dq to move to next pose. + # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). + for pose in poses: + sTt = arm.fk(q) + error = delta(sTt, pose) + J = arm.jacobian(q) + q += np.dot(np.linalg.inv(J), error) + arm.plot(fignum, q) + plt.pause(0.01) + + plt.pause(10) + + +if __name__ == "__main__": + run_example() + unittest.main() diff --git a/python/gtsam_py/python/examples/PlanarSLAMExample.py b/python/gtsam_py/python/examples/PlanarSLAMExample.py new file mode 100644 index 000000000..c84f0f834 --- /dev/null +++ b/python/gtsam_py/python/examples/PlanarSLAMExample.py @@ -0,0 +1,81 @@ +""" +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) + +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 gtsam +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X + +# Create noise models +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 = gtsam.NonlinearFactorGraph() + +# Create the keys corresponding to unknown variables in the factor graph +X1 = X(1) +X2 = X(2) +X3 = X(3) +L1 = L(4) +L2 = L(5) + +# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model +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(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(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 +print("Factor Graph:\n{}".format(graph)) + +# Create (deliberately inaccurate) initial estimate +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 +print("Initial Estimate:\n{}".format(initial_estimate)) + +# 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. +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) +result = optimizer.optimize() +print("\nFinal Result:\n{}".format(result)) + +# Calculate and print marginal covariances for all variables +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/python/gtsam_py/python/examples/Pose2SLAMExample.py b/python/gtsam_py/python/examples/Pose2SLAMExample.py new file mode 100644 index 000000000..680f2209f --- /dev/null +++ b/python/gtsam_py/python/examples/Pose2SLAMExample.py @@ -0,0 +1,97 @@ +""" +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) + +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 + +import numpy as np + +import gtsam + +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + + +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 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), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) +graph.add(gtsam.BetweenFactorPose2( + 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), ODOMETRY_NOISE)) +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 +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)) +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, +# 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() +print("Final Result:\n{}".format(result)) + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +for i in range(1, 6): + print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + +fig = plt.figure(0) +for i in range(1, 6): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) + +plt.axis('equal') +plt.show() diff --git a/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py new file mode 100644 index 000000000..09114370d --- /dev/null +++ b/python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py @@ -0,0 +1,88 @@ +""" +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) + +See LICENSE for the license information + +A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph +and does the optimization. Output is written on a file, in g2o format +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function +import argparse +import math +import numpy as np +import matplotlib.pyplot as plt + +import gtsam +from gtsam.utils import plot + + +def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=np.float) + + +parser = argparse.ArgumentParser( + description="A 2D Pose SLAM example that reads input from g2o, " + "converts it to a factor graph and does the optimization. " + "Output is written on a file, in g2o format") +parser.add_argument('-i', '--input', help='input file g2o format') +parser.add_argument('-o', '--output', + help="the path to the output file with optimized graph") +parser.add_argument('-m', '--maxiter', type=int, + help="maximum number of iterations for optimizer") +parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'], + default="none", help="Type of kernel used") +parser.add_argument("-p", "--plot", action="store_true", + help="Flag to plot results") +args = parser.parse_args() + +g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ + else args.input + +maxIterations = 100 if args.maxiter is None else args.maxiter + +is3D = False + +graph, initial = gtsam.readG2o(g2oFile, is3D) + +assert args.kernel == "none", "Supplied kernel type is not yet implemented" + +# Add prior on the pose having index (key) = 0 +priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) +graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) + +params = gtsam.GaussNewtonParams() +params.setVerbosity("Termination") +params.setMaxIterations(maxIterations) +# parameters.setRelativeErrorTol(1e-5) +# Create the optimizer ... +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) +# ... and optimize +result = optimizer.optimize() + +print("Optimization complete") +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) + + +if args.output is None: + print("\nFactor Graph:\n{}".format(graph)) + print("\nInitial Estimate:\n{}".format(initial)) + print("Final Result:\n{}".format(result)) +else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print ("Done!") + +if args.plot: + resultPoses = gtsam.utilities_extractPose2(result) + for i in range(resultPoses.shape[0]): + plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) + plt.show() diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py new file mode 100644 index 000000000..3c1a54f7b --- /dev/null +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py @@ -0,0 +1,71 @@ +""" + * @file Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the + * Pose3 using InitializePose3 + * @date Jan 17, 2019 + * @author Vikrant Shah based on CPP example by Luca Carlone +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function +import argparse +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +import gtsam +from gtsam.utils import plot + + +def vector6(x, y, z, a, b, c): + """Create 6d double numpy array.""" + return np.array([x, y, z, a, b, c], dtype=np.float) + + +parser = argparse.ArgumentParser( + description="A 3D Pose SLAM example that reads input from g2o, and " + "initializes Pose3") +parser.add_argument('-i', '--input', help='input file g2o format') +parser.add_argument('-o', '--output', + help="the path to the output file with optimized graph") +parser.add_argument("-p", "--plot", action="store_true", + help="Flag to plot results") +args = parser.parse_args() + +g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ + else args.input + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add Prior on the first key +priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, + 1e-4, 1e-4, 1e-4)) + +print("Adding prior to g2o file ") +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) + +params = gtsam.GaussNewtonParams() +params.setVerbosity("Termination") # this will show info about stopping conds +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) +result = optimizer.optimize() +print("Optimization complete") + +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) + +if args.output is None: + print("Final Result:\n{}".format(result)) +else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print ("Done!") + +if args.plot: + resultPoses = gtsam.utilities_allPose3s(result) + for i in range(resultPoses.size()): + plot.plot_pose3(1, resultPoses.atPose3(i)) + plt.show() diff --git a/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py new file mode 100644 index 000000000..02c696905 --- /dev/null +++ b/python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -0,0 +1,35 @@ +""" +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) + +See LICENSE for the license information + +Initialize PoseSLAM with Chordal init +Author: Luca Carlone, Frank Dellaert (python port) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Read graph from file +g2oFile = gtsam.findExampleDataFile("pose3example.txt") + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add prior on the first key. TODO: assumes first key ios z +priorModel = gtsam.noiseModel_Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + +# Initializing Pose3 - chordal relaxation" +initialization = gtsam.InitializePose3.initialize(graph) + +print(initialization) diff --git a/python/gtsam_py/python/examples/PreintegrationExample.py b/python/gtsam_py/python/examples/PreintegrationExample.py new file mode 100644 index 000000000..76520b355 --- /dev/null +++ b/python/gtsam_py/python/examples/PreintegrationExample.py @@ -0,0 +1,140 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +A script validating the Preintegration of IMU measurements +""" + +import math + +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.mplot3d import Axes3D + +import gtsam +from gtsam.utils.plot import plot_pose3 + +IMU_FIG = 1 +POSES_FIG = 2 + + +class PreintegrationExample(object): + + @staticmethod + def defaultParams(g): + """Create default parameters with Z *up* and realistic noise parameters""" + params = gtsam.PreintegrationParams.MakeSharedU(g) + kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW + kAccelSigma = 0.1 / 60 # 10 cm VRW + params.setGyroscopeCovariance( + kGyroSigma ** 2 * np.identity(3, np.float)) + params.setAccelerometerCovariance( + kAccelSigma ** 2 * np.identity(3, np.float)) + params.setIntegrationCovariance( + 0.0000001 ** 2 * np.identity(3, np.float)) + return params + + def __init__(self, twist=None, bias=None, dt=1e-2): + """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" + + # setup interactive plotting + plt.ion() + + # Setup loop as default scenario + if twist is not None: + (W, V) = twist + else: + # default = loop with forward velocity 2m/s, while pitching up + # with angular velocity 30 degree/sec (negative in FLU) + W = np.array([0, -math.radians(30), 0]) + V = np.array([2, 0, 0]) + + self.scenario = gtsam.ConstantTwistScenario(W, V) + self.dt = dt + + self.maxDim = 5 + self.labels = list('xyz') + self.colors = list('rgb') + + # Create runner + self.g = 10 # simple gravity constant + self.params = self.defaultParams(self.g) + + if bias is not None: + self.actualBias = bias + else: + accBias = np.array([0, 0.1, 0]) + gyroBias = np.array([0, 0, 0]) + self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias) + + self.runner = gtsam.ScenarioRunner( + self.scenario, self.params, self.dt, self.actualBias) + + def plotImu(self, t, measuredOmega, measuredAcc): + plt.figure(IMU_FIG) + + # plot angular velocity + omega_b = self.scenario.omega_b(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 1) + plt.scatter(t, omega_b[i], color='k', marker='.') + plt.scatter(t, measuredOmega[i], color=color, marker='.') + plt.xlabel('angular velocity ' + label) + + # plot acceleration in nav + acceleration_n = self.scenario.acceleration_n(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 4) + plt.scatter(t, acceleration_n[i], color=color, marker='.') + plt.xlabel('acceleration in nav ' + label) + + # plot acceleration in body + acceleration_b = self.scenario.acceleration_b(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 7) + plt.scatter(t, acceleration_b[i], color=color, marker='.') + plt.xlabel('acceleration in body ' + label) + + # plot actual specific force, as well as corrupted + actual = self.runner.actualSpecificForce(t) + for i, (label, color) in enumerate(zip(self.labels, self.colors)): + plt.subplot(4, 3, i + 10) + plt.scatter(t, actual[i], color='k', marker='.') + plt.scatter(t, measuredAcc[i], color=color, marker='.') + plt.xlabel('specific force ' + label) + + def plotGroundTruthPose(self, t): + # plot ground truth pose, as well as prediction from integrated IMU measurements + actualPose = self.scenario.pose(t) + plot_pose3(POSES_FIG, actualPose, 0.3) + t = actualPose.translation() + self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) + ax = plt.gca() + ax.set_xlim3d(-self.maxDim, self.maxDim) + ax.set_ylim3d(-self.maxDim, self.maxDim) + ax.set_zlim3d(-self.maxDim, self.maxDim) + + plt.pause(0.01) + + def run(self): + # simulate the loop + T = 12 + for i, t in enumerate(np.arange(0, T, self.dt)): + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + if i % 25 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + self.plotGroundTruthPose(t) + pim = self.runner.integrate(t, self.actualBias, True) + predictedNavState = self.runner.predict(pim, self.actualBias) + plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1) + + plt.ioff() + plt.show() + + +if __name__ == '__main__': + PreintegrationExample().run() diff --git a/python/gtsam_py/python/examples/README.md b/python/gtsam_py/python/examples/README.md new file mode 100644 index 000000000..99bce00e2 --- /dev/null +++ b/python/gtsam_py/python/examples/README.md @@ -0,0 +1,57 @@ +These examples are almost identical to the old handwritten python wrapper +examples. However, there are just some slight name changes, for example +`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc... +Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing. + +# Porting Progress + +| C++ Example Name | Ported | +|-------------------------------------------------------|--------| +| CameraResectioning | | +| CreateSFMExampleData | | +| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython | +| ImuFactorExample2 | X | +| ImuFactorsExample | | +| ISAM2Example_SmartFactor | | +| ISAM2_SmartFactorStereo_IMU | | +| LocalizationExample | impossible? | +| METISOrderingExample | | +| OdometryExample | X | +| PlanarSLAMExample | X | +| Pose2SLAMExample | X | +| Pose2SLAMExampleExpressions | | +| Pose2SLAMExample_g2o | X | +| Pose2SLAMExample_graph | | +| Pose2SLAMExample_graphviz | | +| Pose2SLAMExample_lago | lago not exposed through cython | +| Pose2SLAMStressTest | | +| Pose2SLAMwSPCG | | +| Pose3SLAMExample_changeKeys | | +| Pose3SLAMExampleExpressions_BearingRangeWithTransform | | +| Pose3SLAMExample_g2o | X | +| Pose3SLAMExample_initializePose3Chordal | | +| Pose3SLAMExample_initializePose3Gradient | | +| RangeISAMExample_plaza2 | | +| SelfCalibrationExample | | +| SFMExample_bal_COLAMD_METIS | | +| SFMExample_bal | | +| SFMExample | X | +| SFMExampleExpressions_bal | | +| SFMExampleExpressions | | +| SFMExample_SmartFactor | | +| SFMExample_SmartFactorPCG | | +| SimpleRotation | X | +| SolverComparer | | +| StereoVOExample | | +| StereoVOExample_large | | +| TimeTBB | | +| UGM_chain | discrete functionality not exposed | +| UGM_small | discrete functionality not exposed | +| VisualISAM2Example | X | +| VisualISAMExample | X | + +Extra Examples (with no C++ equivalent) +- PlanarManipulatorExample +- SFMData diff --git a/python/gtsam_py/python/examples/SFMExample.py b/python/gtsam_py/python/examples/SFMExample.py new file mode 100644 index 000000000..e02def2f9 --- /dev/null +++ b/python/gtsam_py/python/examples/SFMExample.py @@ -0,0 +1,121 @@ +""" +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 + +A structure-from-motion problem on a simulated dataset +""" +from __future__ import print_function + +import gtsam +import matplotlib.pyplot as plt +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X +from gtsam.examples import SFMdata +from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, + GenericProjectionFactorCal3_S2, Marginals, + NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, + Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, + SimpleCamera, Values) +from gtsam.utils import plot + + +def main(): + """ + Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). + + Each variable in the system (poses and landmarks) must be identified with a unique key. + We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). + Here we will use Symbols + + In GTSAM, measurement functions are represented as 'factors'. Several common factors + have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. + Here we will use Projection factors to model the camera's landmark observations. + Also, we will initialize the robot at some location using a Prior factor. + + When the factors are created, we will add them to a Factor Graph. As the factors we are using + are nonlinear factors, we will need a Nonlinear Factor Graph. + + Finally, once all of the factors have been added to our factor graph, we will want to + solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. + GTSAM includes several nonlinear optimizers to perform this step. Here we will use a + trust-region method known as Powell's Degleg + + The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the + nonlinear functions around an initial linearization point, then solve the linear system + to update the linearization point. This happens repeatedly until the solver converges + to a consistent set of variable values. This requires us to specify an initial guess + for each variable, held in a Values container. + """ + + # Define the camera calibration parameters + K = 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 a factor graph + graph = NonlinearFactorGraph() + + # Add a prior on pose x1. This indirectly specifies where the origin is. + # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(X(0), poses[0], pose_noise) + graph.push_back(factor) + + # Simulated measurements from each camera pose, adding them to the factor graph + for i, pose in enumerate(poses): + camera = PinholeCameraCal3_S2(pose, K) + for j, point in enumerate(points): + measurement = camera.project(point) + factor = GenericProjectionFactorCal3_S2( + measurement, measurement_noise, X(i), L(j), K) + graph.push_back(factor) + + # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance + # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(L(0), points[0], point_noise) + graph.push_back(factor) + graph.print_('Factor Graph:\n') + + # Create the data structure to hold the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initial_estimate = Values() + for i, pose in enumerate(poses): + transformed_pose = pose.retract(0.1*np.random.randn(6,1)) + initial_estimate.insert(X(i), transformed_pose) + for j, point in enumerate(points): + transformed_point = Point3(point.vector() + 0.1*np.random.randn(3)) + initial_estimate.insert(L(j), transformed_point) + initial_estimate.print_('Initial Estimates:\n') + + # Optimize the graph and print results + params = gtsam.DoglegParams() + params.setVerbosity('TERMINATION') + optimizer = DoglegOptimizer(graph, initial_estimate, params) + print('Optimizing:') + result = optimizer.optimize() + result.print_('Final results:\n') + print('initial error = {}'.format(graph.error(initial_estimate))) + print('final error = {}'.format(graph.error(result))) + + marginals = Marginals(graph, result) + plot.plot_3d_points(1, result, marginals=marginals) + plot.plot_trajectory(1, result, marginals=marginals, scale=8) + plot.set_axes_equal(1) + plt.show() + +if __name__ == '__main__': + main() diff --git a/python/gtsam_py/python/examples/SFMdata.py b/python/gtsam_py/python/examples/SFMdata.py new file mode 100644 index 000000000..c586f7e52 --- /dev/null +++ b/python/gtsam_py/python/examples/SFMdata.py @@ -0,0 +1,39 @@ +""" +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 + +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 = 40.0 + height = 10.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), height) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) + poses.append(camera.pose()) + return poses diff --git a/python/gtsam_py/python/examples/SimpleRotation.py b/python/gtsam_py/python/examples/SimpleRotation.py new file mode 100644 index 000000000..4e82d3778 --- /dev/null +++ b/python/gtsam_py/python/examples/SimpleRotation.py @@ -0,0 +1,85 @@ +""" +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 + +This example will perform a relatively trivial optimization on +a single variable with a single factor. +""" + +import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X + + +def main(): + """ + Step 1: Create a factor to express a unary constraint + + The "prior" in this case is the measurement from a sensor, + with a model of the noise on the measurement. + + The "Key" created here is a label used to associate parts of the + state (stored in "RotValues") with particular factors. They require + an index to allow for lookup, and should be unique. + + In general, creating a factor requires: + - A key or set of keys labeling the variables that are acted upon + - A measurement value + - A measurement model with the correct dimensionality for the factor + """ + prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) + prior.print_('goal angle') + model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) + key = X(1) + factor = gtsam.PriorFactorRot2(key, prior, model) + + """ + Step 2: Create a graph container and add the factor to it + + Before optimizing, all factors need to be added to a Graph container, + which provides the necessary top-level functionality for defining a + system of constraints. + + In this case, there is only one factor, but in a practical scenario, + many more factors would be added. + """ + graph = gtsam.NonlinearFactorGraph() + graph.push_back(factor) + graph.print_('full graph') + + """ + Step 3: Create an initial estimate + + An initial estimate of the solution for the system is necessary to + start optimization. This system state is the "Values" instance, + which is similar in structure to a dictionary, in that it maps + keys (the label created in step 1) to specific values. + + The initial estimate provided to optimization will be used as + a linearization point for optimization, so it is important that + all of the variables in the graph have a corresponding value in + this structure. + """ + initial = gtsam.Values() + initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) + initial.print_('initial estimate') + + """ + Step 4: Optimize + + After formulating the problem with a graph of constraints + and an initial estimate, executing optimization is as simple + as calling a general optimization function with the graph and + initial estimate. This will yield a new RotValues structure + with the final state of the optimization. + """ + result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() + result.print_('final result') + + +if __name__ == '__main__': + main() diff --git a/python/gtsam_py/python/examples/VisualISAM2Example.py b/python/gtsam_py/python/examples/VisualISAM2Example.py new file mode 100644 index 000000000..49e6ca95c --- /dev/null +++ b/python/gtsam_py/python/examples/VisualISAM2Example.py @@ -0,0 +1,154 @@ +""" +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) + +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 + +import gtsam +import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X +from gtsam.examples import SFMdata +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 + + +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.1, 0.1, 0.1, 0.3, 0.3, 0.3])) # 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/python/gtsam_py/python/examples/VisualISAMExample.py b/python/gtsam_py/python/examples/VisualISAMExample.py new file mode 100644 index 000000000..5cc37867b --- /dev/null +++ b/python/gtsam_py/python/examples/VisualISAMExample.py @@ -0,0 +1,103 @@ +""" +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 + +A visualSLAM example for the structure-from-motion problem on a simulated dataset +This version uses iSAM to solve the problem incrementally +""" + +from __future__ import print_function + +import numpy as np +import gtsam +from gtsam.examples import SFMdata +from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, + NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, + PinholeCameraCal3_S2, Values) +from gtsam import symbol_shorthand_L as L +from gtsam import symbol_shorthand_X as X + + +def main(): + """ + 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 + """ + + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + camera_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 a NonlinearISAM object which will relinearize and reorder the variables + # every "reorderInterval" updates + isam = NonlinearISAM(reorderInterval=3) + + # Create a Factor Graph and Values to hold the new data + graph = NonlinearFactorGraph() + initial_estimate = Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + camera = PinholeCameraCal3_S2(pose, K) + # Add factors for each landmark observation + for j, point in enumerate(points): + measurement = camera.project(point) + factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K) + graph.push_back(factor) + + # Intentionally initialize the variables off from the ground truth + noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) + initial_xi = pose.compose(noise) + + # Add an initial guess for the current pose + initial_estimate.insert(X(i), initial_xi) + + # 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, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(X(0), poses[0], pose_noise) + graph.push_back(factor) + + # Add a prior on landmark l0 + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(L(0), points[0], point_noise) + graph.push_back(factor) + + # Add initial guesses to all observed landmarks + noise = np.array([-0.25, 0.20, 0.15]) + for j, point in enumerate(points): + # Intentionally initialize the variables off from the ground truth + initial_lj = points[j].vector() + noise + initial_estimate.insert(L(j), Point3(initial_lj)) + else: + # Update iSAM with the new factors + isam.update(graph, initial_estimate) + current_estimate = isam.estimate() + print('*' * 50) + print('Frame {}:'.format(i)) + current_estimate.print_('Current estimate: ') + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initial_estimate.clear() + + +if __name__ == '__main__': + main() diff --git a/python/gtsam_py/python/examples/__init__.py b/python/gtsam_py/python/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/gtsam_py/python/tests/__init__.py b/python/gtsam_py/python/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/gtsam_py/python/tests/testScenarioRunner.py b/python/gtsam_py/python/tests/testScenarioRunner.py new file mode 100644 index 000000000..97a97b0ec --- /dev/null +++ b/python/gtsam_py/python/tests/testScenarioRunner.py @@ -0,0 +1,47 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +ScenarioRunner unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import math +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestScenarioRunner(GtsamTestCase): + def setUp(self): + self.g = 10 # simple gravity constant + + def test_loop_runner(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) + + dt = 0.1 + params = gtsam.PreintegrationParams.MakeSharedU(self.g) + bias = gtsam.imuBias_ConstantBias() + runner = gtsam.ScenarioRunner( + scenario, params, dt, bias) + + # Test specific force at time 0: a is pointing up + t = 0.0 + a = w * v + np.testing.assert_almost_equal( + np.array([0, 0, a + self.g]), runner.actualSpecificForce(t)) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Cal3Unified.py b/python/gtsam_py/python/tests/test_Cal3Unified.py new file mode 100644 index 000000000..fbf5f3565 --- /dev/null +++ b/python/gtsam_py/python/tests/test_Cal3Unified.py @@ -0,0 +1,38 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestCal3Unified(GtsamTestCase): + + def test_Cal3Unified(self): + K = gtsam.Cal3Unified() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fx(), 1.) + + def test_retract(self): + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') + actual = K.retract(d) + self.gtsamAssertEquals(actual, expected) + np.testing.assert_allclose(d, K.localCoordinates(actual)) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_FrobeniusFactor.py b/python/gtsam_py/python/tests/test_FrobeniusFactor.py new file mode 100644 index 000000000..f3f5354bb --- /dev/null +++ b/python/gtsam_py/python/tests/test_FrobeniusFactor.py @@ -0,0 +1,56 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +FrobeniusFactor unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error, invalid-name +import unittest + +import numpy as np +from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, + FrobeniusWormholeFactor, SOn) + +id = SO4() +v1 = np.array([0, 0, 0, 0.1, 0, 0]) +Q1 = SO4.Expmap(v1) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q2 = SO4.Expmap(v2) + + +class TestFrobeniusFactorSO4(unittest.TestCase): + """Test FrobeniusFactor factors.""" + + def test_frobenius_factor(self): + """Test creation of a factor that calculates the Frobenius norm.""" + factor = FrobeniusFactorSO4(1, 2) + actual = factor.evaluateError(Q1, Q2) + expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,)) + np.testing.assert_array_equal(actual, expected) + + def test_frobenius_between_factor(self): + """Test creation of a Frobenius BetweenFactor.""" + factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2)) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((16,)) + np.testing.assert_array_almost_equal(actual, expected) + + def test_frobenius_wormhole_factor(self): + """Test creation of a factor that calculates Shonan error.""" + R1 = SO3.Expmap(v1[3:]) + R2 = SO3.Expmap(v2[3:]) + factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4) + I4 = SOn(4) + Q1 = I4.retract(v1) + Q2 = I4.retract(v2) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((12,)) + np.testing.assert_array_almost_equal(actual, expected, decimal=4) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py new file mode 100644 index 000000000..983825d8b --- /dev/null +++ b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py @@ -0,0 +1,92 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Linear Factor Graphs. +Author: Frank Dellaert & Gerry Chen +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +import numpy as np +from gtsam import symbol_shorthand_X as X +from gtsam.utils.test_case import GtsamTestCase + + +def create_graph(): + """Create a basic linear factor graph for testing""" + graph = gtsam.GaussianFactorGraph() + + x0 = X(0) + x1 = X(1) + x2 = X(2) + + BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) + PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1)) + + graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) + graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) + graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) + + return graph, (x0, x1, x2) + +class TestGaussianFactorGraph(GtsamTestCase): + """Tests for Gaussian Factor Graphs.""" + + def test_fg(self): + """Test solving a linear factor graph""" + graph, X = create_graph() + result = graph.optimize() + + EXPECTEDX = [0, 1, 3] + + # check solutions + self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8) + self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8) + self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8) + + def test_convertNonlinear(self): + """Test converting a linear factor graph to a nonlinear one""" + graph, X = create_graph() + + EXPECTEDM = [1, 2, 3] + + # create nonlinear factor graph for marginalization + nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph) + optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values()) + nlresult = optimizer.optimizeSafely() + + # marginalize + marginals = gtsam.Marginals(nfg, nlresult) + m = [marginals.marginalCovariance(x) for x in X] + + # check linear marginalizations + self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + + def test_linearMarginalization(self): + """Marginalize a linear factor graph""" + graph, X = create_graph() + result = graph.optimize() + + EXPECTEDM = [1, 2, 3] + + # linear factor graph marginalize + marginals = gtsam.Marginals(graph, result) + m = [marginals.marginalCovariance(x) for x in X] + + # check linear marginalizations + self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) + self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_py/python/tests/test_JacobianFactor.py b/python/gtsam_py/python/tests/test_JacobianFactor.py new file mode 100644 index 000000000..04433492b --- /dev/null +++ b/python/gtsam_py/python/tests/test_JacobianFactor.py @@ -0,0 +1,92 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +JacobianFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestJacobianFactor(GtsamTestCase): + + def test_eliminate(self): + # Recommended way to specify a matrix (see cython/README) + Ax2 = np.array( + [[-5., 0.], + [+0., -5.], + [10., 0.], + [+0., 10.]], order='F') + + # This is good too + Al1 = np.array( + [[5, 0], + [0, 5], + [0, 0], + [0, 0]], dtype=float, order = 'F') + + # Not recommended for performance reasons, but should still work + # as the wrapper should convert it to the correct type and storage order + Ax1 = np.array( + [[0, 0], # f4 + [0, 0], # f4 + [-10, 0], # f2 + [0, -10]]) # f2 + + x2 = 1 + l1 = 2 + x1 = 3 + + # the RHS + b2 = np.array([-1., 1.5, 2., -1.]) + sigmas = np.array([1., 1., 1., 1.]) + model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas) + combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) + + # eliminate the first variable (x2) in the combined factor, destructive + # ! + ord = gtsam.Ordering() + ord.push_back(x2) + actualCG, lf = combined.eliminate(ord) + + # create expected Conditional Gaussian + R11 = np.array([[11.1803, 0.00], + [0.00, 11.1803]]) + S12 = np.array([[-2.23607, 0.00], + [+0.00, -2.23607]]) + S13 = np.array([[-8.94427, 0.00], + [+0.00, -8.94427]]) + d = np.array([2.23607, -1.56525]) + expectedCG = gtsam.GaussianConditional( + x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) + # check if the result matches + self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) + + # the expected linear factor + Bl1 = np.array([[4.47214, 0.00], + [0.00, 4.47214]]) + + Bx1 = np.array( + # x1 + [[-4.47214, 0.00], + [+0.00, -4.47214]]) + + # the RHS + b1 = np.array([0.0, 0.894427]) + + model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.])) + expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) + + # check if the result matches the combined (reduced) factor + self.gtsamAssertEquals(lf, expectedLF, 1e-4) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_KalmanFilter.py b/python/gtsam_py/python/tests/test_KalmanFilter.py new file mode 100644 index 000000000..94c41df72 --- /dev/null +++ b/python/gtsam_py/python/tests/test_KalmanFilter.py @@ -0,0 +1,83 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestKalmanFilter(GtsamTestCase): + + def test_KalmanFilter(self): + F = np.eye(2) + B = np.eye(2) + u = np.array([1.0, 0.0]) + modelQ = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + Q = 0.01 * np.eye(2) + H = np.eye(2) + z1 = np.array([1.0, 0.0]) + z2 = np.array([2.0, 0.0]) + z3 = np.array([3.0, 0.0]) + modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + R = 0.01 * np.eye(2) + + # Create the set of expected output TestValues + expected0 = np.array([0.0, 0.0]) + P00 = 0.01 * np.eye(2) + + expected1 = np.array([1.0, 0.0]) + P01 = P00 + Q + I11 = np.linalg.inv(P01) + np.linalg.inv(R) + + expected2 = np.array([2.0, 0.0]) + P12 = np.linalg.inv(I11) + Q + I22 = np.linalg.inv(P12) + np.linalg.inv(R) + + expected3 = np.array([3.0, 0.0]) + P23 = np.linalg.inv(I22) + Q + I33 = np.linalg.inv(P23) + np.linalg.inv(R) + + # Create an KalmanFilter object + KF = gtsam.KalmanFilter(n=2) + + # Create the Kalman Filter initialization point + x_initial = np.array([0.0, 0.0]) + P_initial = 0.01 * np.eye(2) + + # Create an KF object + state = KF.init(x_initial, P_initial) + self.assertTrue(np.allclose(expected0, state.mean())) + self.assertTrue(np.allclose(P00, state.covariance())) + + # Run iteration 1 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected1, state.mean())) + self.assertTrue(np.allclose(P01, state.covariance())) + state = KF.update(state, H, z1, modelR) + self.assertTrue(np.allclose(expected1, state.mean())) + self.assertTrue(np.allclose(I11, state.information())) + + # Run iteration 2 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected2, state.mean())) + state = KF.update(state, H, z2, modelR) + self.assertTrue(np.allclose(expected2, state.mean())) + + # Run iteration 3 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected3, state.mean())) + state = KF.update(state, H, z3, modelR) + self.assertTrue(np.allclose(expected3, state.mean())) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_KarcherMeanFactor.py b/python/gtsam_py/python/tests/test_KarcherMeanFactor.py new file mode 100644 index 000000000..6976decc1 --- /dev/null +++ b/python/gtsam_py/python/tests/test_KarcherMeanFactor.py @@ -0,0 +1,80 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KarcherMeanFactor unit tests. +Author: Frank Dellaert +""" + +# pylint: disable=invalid-name, no-name-in-module, no-member + +import unittest + +import gtsam +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +KEY = 0 +MODEL = gtsam.noiseModel_Unit.Create(3) + + +def find_Karcher_mean_Rot3(rotations): + """Find the Karcher mean of given values.""" + # Cost function C(R) = \sum PriorFactor(R_i)::error(R) + # No closed form solution. + graph = gtsam.NonlinearFactorGraph() + for R in rotations: + graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) + initial = gtsam.Values() + initial.insert(KEY, gtsam.Rot3()) + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + return result.atRot3(KEY) + + +# Rot3 version +R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0])) + + +class TestKarcherMean(GtsamTestCase): + + def test_find(self): + # Check that optimizing for Karcher mean (which minimizes Between distance) + # gets correct result. + rotations = {R, R.inverse()} + expected = gtsam.Rot3() + actual = find_Karcher_mean_Rot3(rotations) + self.gtsamAssertEquals(expected, actual) + + def test_factor(self): + """Check that the InnerConstraint factor leaves the mean unchanged.""" + # Make a graph with two variables, one between, and one InnerConstraint + # The optimal result should satisfy the between, while moving the other + # variable to make the mean the same as before. + # Mean of R and R' is identity. Let's make a BetweenFactor making R21 = + # R*R*R, i.e. geodesic length is 3 rather than 2. + graph = gtsam.NonlinearFactorGraph() + R12 = R.compose(R.compose(R)) + graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) + keys = gtsam.KeyVector() + keys.push_back(1) + keys.push_back(2) + graph.add(gtsam.KarcherMeanFactorRot3(keys)) + + initial = gtsam.Values() + initial.insert(1, R.inverse()) + initial.insert(2, R) + expected = find_Karcher_mean_Rot3([R, R.inverse()]) + + result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() + actual = find_Karcher_mean_Rot3( + [result.atRot3(1), result.atRot3(2)]) + self.gtsamAssertEquals(expected, actual) + self.gtsamAssertEquals( + R12, result.atRot3(1).between(result.atRot3(2))) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_LocalizationExample.py b/python/gtsam_py/python/tests/test_LocalizationExample.py new file mode 100644 index 000000000..6ce65f087 --- /dev/null +++ b/python/gtsam_py/python/tests/test_LocalizationExample.py @@ -0,0 +1,64 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Localization unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestLocalizationExample(GtsamTestCase): + + def test_LocalizationExample(self): + # Create the graph (defined in pose2SLAM.h, derived from + # NonlinearFactorGraph) + graph = gtsam.NonlinearFactorGraph() + + # Add two odometry factors + # create a measurement for both factors (the same in this case) + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta + graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise)) + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) + + # Add three "GPS" measurements + # We use Pose2 Priors here with high variance on theta + groundTruth = gtsam.Values() + groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0)) + groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0)) + groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0)) + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) + for i in range(3): + graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = gtsam.Marginals(graph, result) + P = [None] * result.size() + for i in range(0, result.size()): + pose_i = result.atPose2(i) + self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) + P[i] = marginals.marginalCovariance(i) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py new file mode 100644 index 000000000..985dc30a2 --- /dev/null +++ b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py @@ -0,0 +1,83 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, + GaussNewtonParams, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, PCGSolverParameters, + DummyPreconditionerParameters, NonlinearFactorGraph, Ordering, + Point2, PriorFactorPoint2, Values) +from gtsam.utils.test_case import GtsamTestCase + +KEY1 = 1 +KEY2 = 2 + + +class TestScenario(GtsamTestCase): + def test_optimize(self): + """Do trivial test with three optimizer variants.""" + fg = NonlinearFactorGraph() + model = gtsam.noiseModel_Unit.Create(2) + fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) + + # test error at minimum + xstar = Point2(0, 0) + optimal_values = Values() + optimal_values.insert(KEY1, xstar) + self.assertEqual(0.0, fg.error(optimal_values), 0.0) + + # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + x0 = Point2(3, 3) + initial_values = Values() + initial_values.insert(KEY1, x0) + self.assertEqual(9.0, fg.error(initial_values), 1e-3) + + # optimize parameters + ordering = Ordering() + ordering.push_back(KEY1) + + # Gauss-Newton + gnParams = GaussNewtonParams() + gnParams.setOrdering(ordering) + actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() + self.assertAlmostEqual(0, fg.error(actual1)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setOrdering(ordering) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setLinearSolverType("ITERATIVE") + cgParams = PCGSolverParameters() + cgParams.setPreconditionerParams(DummyPreconditionerParameters()) + lmParams.setIterativeParams(cgParams) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Dogleg + dlParams = DoglegParams() + dlParams.setOrdering(ordering) + actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() + self.assertAlmostEqual(0, fg.error(actual3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_OdometryExample.py b/python/gtsam_py/python/tests/test_OdometryExample.py new file mode 100644 index 000000000..c8ea95588 --- /dev/null +++ b/python/gtsam_py/python/tests/test_OdometryExample.py @@ -0,0 +1,59 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Odometry unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestOdometryExample(GtsamTestCase): + + def test_OdometryExample(self): + # Create the graph (defined in pose2SLAM.h, derived from + # NonlinearFactorGraph) + graph = gtsam.NonlinearFactorGraph() + + # Add a Gaussian prior on pose x_1 + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin + priorNoise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta + # add directly to graph + graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + + # Add two odometry factors + # create a measurement for both factors (the same in this case) + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) + graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + marginals = gtsam.Marginals(graph, result) + marginals.marginalCovariance(1) + + # Check first pose equality + pose_1 = result.atPose2(1) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_PlanarSLAMExample.py b/python/gtsam_py/python/tests/test_PlanarSLAMExample.py new file mode 100644 index 000000000..ae813d35c --- /dev/null +++ b/python/gtsam_py/python/tests/test_PlanarSLAMExample.py @@ -0,0 +1,78 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PlanarSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPlanarSLAM(GtsamTestCase): + + def test_PlanarSLAM(self): + # Assumptions + # - All values are axis aligned + # - Robot poses are facing along the X axis (horizontal, to the right in images) + # - We have full odometry for measurements + # - The robot is on a grid, moving 2 meters each step + + # Create graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + # Add prior + # gaussian for prior + 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])) + # add directly to graph + graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + + # Add odometry + # general noisemodel for odometry + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2( + 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + + # Add pose constraint + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) + initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = gtsam.Marginals(graph, result) + P = marginals.marginalCovariance(1) + + pose_1 = result.atPose2(1) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) + + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Pose2.py b/python/gtsam_py/python/tests/test_Pose2.py new file mode 100644 index 000000000..9652b594a --- /dev/null +++ b/python/gtsam_py/python/tests/test_Pose2.py @@ -0,0 +1,32 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2(GtsamTestCase): + """Test selected Pose2 methods.""" + + def test_adjoint(self): + """Test adjoint method.""" + xi = np.array([1, 2, 3]) + expected = np.dot(Pose2.adjointMap_(xi), xi) + actual = Pose2.adjoint_(xi, xi) + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Pose2SLAMExample.py b/python/gtsam_py/python/tests/test_Pose2SLAMExample.py new file mode 100644 index 000000000..a79b6b18c --- /dev/null +++ b/python/gtsam_py/python/tests/test_Pose2SLAMExample.py @@ -0,0 +1,76 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2SLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2SLAMExample(GtsamTestCase): + + def test_Pose2SLAMExample(self): + # Assumptions + # - All values are axis aligned + # - Robot poses are facing along the X axis (horizontal, to the right in images) + # - We have full odometry for measurements + # - The robot is on a grid, moving 2 meters each step + + # Create graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + # Add prior + # gaussian for prior + 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])) + # add directly to graph + graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + + # Add odometry + # general noisemodel for odometry + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2( + 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + + # Add pose constraint + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) + + # Initialize to noisy points + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) + initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = gtsam.Marginals(graph, result) + P = marginals.marginalCovariance(1) + + pose_1 = result.atPose2(1) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Pose3.py b/python/gtsam_py/python/tests/test_Pose3.py new file mode 100644 index 000000000..138f1ff13 --- /dev/null +++ b/python/gtsam_py/python/tests/test_Pose3.py @@ -0,0 +1,70 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose3 unit tests. +Author: Frank Dellaert, Duy Nguyen Ta +""" +# pylint: disable=no-name-in-module +import math +import unittest + +import numpy as np + +import gtsam +from gtsam import Point3, Pose3, Rot3 +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose3(GtsamTestCase): + """Test selected Pose3 methods.""" + + def test_between(self): + """Test between method.""" + T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2)) + T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) + expected = T2.inverse().compose(T3) + actual = T2.between(T3) + self.gtsamAssertEquals(actual, expected, 1e-6) + + def test_transform_to(self): + """Test transformTo method.""" + transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) + actual = transform.transformTo(Point3(3, 2, 10)) + expected = Point3(2, 1, 10) + self.gtsamAssertEquals(actual, expected, 1e-6) + + def test_range(self): + """Test range method.""" + l1 = Point3(1, 0, 0) + l2 = Point3(1, 1, 0) + x1 = Pose3() + + xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)) + xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + + # establish range is indeed zero + self.assertEqual(1, x1.range(point=l1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0), x1.range(point=l2)) + + # establish range is indeed zero + self.assertEqual(1, x1.range(pose=xl1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) + + def test_adjoint(self): + """Test adjoint method.""" + xi = np.array([1, 2, 3, 4, 5, 6]) + expected = np.dot(Pose3.adjointMap_(xi), xi) + actual = Pose3.adjoint_(xi, xi) + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Pose3SLAMExample.py b/python/gtsam_py/python/tests/test_Pose3SLAMExample.py new file mode 100644 index 000000000..1e9eaac67 --- /dev/null +++ b/python/gtsam_py/python/tests/test_Pose3SLAMExample.py @@ -0,0 +1,60 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PoseSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.utils.circlePose3 import * + + +class TestPose3SLAMExample(GtsamTestCase): + + def test_Pose3SLAMExample(self): + # Create a hexagon of poses + hexagon = circlePose3(6, 1.0) + p0 = hexagon.atPose3(0) + p1 = hexagon.atPose3(1) + + # create a Pose graph with one equality constraint and one measurement + fg = gtsam.NonlinearFactorGraph() + fg.add(gtsam.NonlinearEqualityPose3(0, p0)) + delta = p0.between(p1) + covariance = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) + fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance)) + + # Create initial config + initial = gtsam.Values() + s = 0.10 + initial.insert(0, p0) + initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) + initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1))) + initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1))) + initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1))) + initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1))) + + # optimize + optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial) + result = optimizer.optimizeSafely() + + pose_1 = result.atPose3(1) + self.gtsamAssertEquals(pose_1, p1, 1e-4) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_PriorFactor.py b/python/gtsam_py/python/tests/test_PriorFactor.py new file mode 100644 index 000000000..66207b800 --- /dev/null +++ b/python/gtsam_py/python/tests/test_PriorFactor.py @@ -0,0 +1,61 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PriorFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPriorFactor(GtsamTestCase): + + def test_PriorFactor(self): + values = gtsam.Values() + + key = 5 + priorPose3 = gtsam.Pose3() + model = gtsam.noiseModel_Unit.Create(6) + factor = gtsam.PriorFactorPose3(key, priorPose3, model) + values.insert(key, priorPose3) + self.assertEqual(factor.error(values), 0) + + key = 3 + priorVector = np.array([0., 0., 0.]) + model = gtsam.noiseModel_Unit.Create(3) + factor = gtsam.PriorFactorVector(key, priorVector, model) + values.insert(key, priorVector) + self.assertEqual(factor.error(values), 0) + + def test_AddPrior(self): + """ + Test adding prior factors directly to factor graph via the .addPrior method. + """ + # define factor graph + graph = gtsam.NonlinearFactorGraph() + + # define and add Pose3 prior + key = 5 + priorPose3 = gtsam.Pose3() + model = gtsam.noiseModel_Unit.Create(6) + graph.addPriorPose3(key, priorPose3, model) + self.assertEqual(graph.size(), 1) + + # define and add Vector prior + key = 3 + priorVector = np.array([0., 0., 0.]) + model = gtsam.noiseModel_Unit.Create(3) + graph.addPriorVector(key, priorVector, model) + self.assertEqual(graph.size(), 2) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_SFMExample.py b/python/gtsam_py/python/tests/test_SFMExample.py new file mode 100644 index 000000000..e8fa46186 --- /dev/null +++ b/python/gtsam_py/python/tests/test_SFMExample.py @@ -0,0 +1,82 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SFM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +import gtsam.utils.visual_data_generator as generator +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase + + +class TestSFMExample(GtsamTestCase): + + def test_SFMExample(self): + options = generator.Options() + options.triangle = False + options.nrCameras = 10 + + [data, truth] = generator.generate_data(options) + + measurementNoiseSigma = 1.0 + pointNoiseSigma = 0.1 + poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) + + graph = gtsam.NonlinearFactorGraph() + + # Add factors for all measurements + measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma) + for i in range(len(data.Z)): + for k in range(len(data.Z[i])): + j = data.J[i][k] + graph.add(gtsam.GenericProjectionFactorCal3_S2( + data.Z[i][k], measurementNoise, + symbol(ord('x'), i), symbol(ord('p'), j), data.K)) + + posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) + graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0), + truth.cameras[0].pose(), posePriorNoise)) + pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma) + graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0), + truth.points[0], pointPriorNoise)) + + # Initial estimate + initialEstimate = gtsam.Values() + for i in range(len(truth.cameras)): + pose_i = truth.cameras[i].pose() + initialEstimate.insert(symbol(ord('x'), i), pose_i) + for j in range(len(truth.points)): + point_j = truth.points[j] + initialEstimate.insert(symbol(ord('p'), j), point_j) + + # Optimization + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + for i in range(5): + optimizer.iterate() + result = optimizer.values() + + # Marginalization + marginals = gtsam.Marginals(graph, result) + marginals.marginalCovariance(symbol(ord('p'), 0)) + marginals.marginalCovariance(symbol(ord('x'), 0)) + + # Check optimized results, should be equal to ground truth + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('p'), j)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_SO4.py b/python/gtsam_py/python/tests/test_SO4.py new file mode 100644 index 000000000..648bd1710 --- /dev/null +++ b/python/gtsam_py/python/tests/test_SO4.py @@ -0,0 +1,59 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SO4 unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error +import unittest + +import numpy as np +from gtsam import SO4 + +I4 = SO4() +v1 = np.array([0, 0, 0, .1, 0, 0]) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q1 = SO4.Expmap(v1) +Q2 = SO4.Expmap(v2) + + +class TestSO4(unittest.TestCase): + """Test selected SO4 methods.""" + + def test_constructor(self): + """Construct from matrix.""" + matrix = np.eye(4) + so4 = SO4(matrix) + self.assertIsInstance(so4, SO4) + + def test_retract(self): + """Test retraction to manifold.""" + v = np.zeros((6,), np.float) + actual = I4.retract(v) + self.assertTrue(actual.equals(I4, 1e-9)) + + def test_local(self): + """Check localCoordinates for trivial case.""" + v0 = np.zeros((6,), np.float) + actual = I4.localCoordinates(I4) + np.testing.assert_array_almost_equal(actual, v0) + + def test_compose(self): + """Check compose works in subgroup.""" + expected = SO4.Expmap(2*v1) + actual = Q1.compose(Q1) + self.assertTrue(actual.equals(expected, 1e-9)) + + def test_vec(self): + """Check wrapping of vec.""" + expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) + actual = I4.vec() + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_SOn.py b/python/gtsam_py/python/tests/test_SOn.py new file mode 100644 index 000000000..7599363e2 --- /dev/null +++ b/python/gtsam_py/python/tests/test_SOn.py @@ -0,0 +1,59 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Dynamic SO(n) unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error +import unittest + +import numpy as np +from gtsam import SOn + +I4 = SOn(4) +v1 = np.array([0, 0, 0, .1, 0, 0]) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q1 = I4.retract(v1) +Q2 = I4.retract(v2) + + +class TestSO4(unittest.TestCase): + """Test selected SOn methods.""" + + def test_constructor(self): + """Construct from matrix.""" + matrix = np.eye(4) + so4 = SOn.FromMatrix(matrix) + self.assertIsInstance(so4, SOn) + + def test_retract(self): + """Test retraction to manifold.""" + v = np.zeros((6,), np.float) + actual = I4.retract(v) + self.assertTrue(actual.equals(I4, 1e-9)) + + def test_local(self): + """Check localCoordinates for trivial case.""" + v0 = np.zeros((6,), np.float) + actual = I4.localCoordinates(I4) + np.testing.assert_array_almost_equal(actual, v0) + + def test_compose(self): + """Check compose works in subgroup.""" + expected = I4.retract(2*v1) + actual = Q1.compose(Q1) + self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate + + def test_vec(self): + """Check wrapping of vec.""" + expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]) + actual = I4.vec() + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Scenario.py b/python/gtsam_py/python/tests/test_Scenario.py new file mode 100644 index 000000000..09601fba5 --- /dev/null +++ b/python/gtsam_py/python/tests/test_Scenario.py @@ -0,0 +1,53 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Scenario unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +from __future__ import print_function + +import math +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + +# pylint: disable=invalid-name, E1101 + + +class TestScenario(GtsamTestCase): + 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.gtsamAssertEquals(gtsam.Point3( + 0, 0, 2.0 * R), T30.translation(), 1e-9) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_py/python/tests/test_SimpleCamera.py b/python/gtsam_py/python/tests/test_SimpleCamera.py new file mode 100644 index 000000000..a3654a5f1 --- /dev/null +++ b/python/gtsam_py/python/tests/test_SimpleCamera.py @@ -0,0 +1,45 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PinholeCameraCal3_S2 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import math +import unittest + +import numpy as np + +import gtsam +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 +from gtsam.utils.test_case import GtsamTestCase + +K = Cal3_S2(625, 625, 0, 0, 0) + +class TestSimpleCamera(GtsamTestCase): + + def test_constructor(self): + pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) + camera = PinholeCameraCal3_S2(pose1, K) + self.gtsamAssertEquals(camera.calibration(), K, 1e-9) + self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) + + def test_level2(self): + # Create a level camera, looking in Y-direction + pose2 = Pose2(0.4,0.3,math.pi/2.0) + camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1) + + # expected + x = Point3(1,0,0) + y = Point3(0,0,-1) + z = Point3(0,1,0) + wRc = Rot3(x,y,z) + expected = Pose3(wRc,Point3(0.4,0.3,0.1)) + self.gtsamAssertEquals(camera.pose(), expected, 1e-9) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_StereoVOExample.py b/python/gtsam_py/python/tests/test_StereoVOExample.py new file mode 100644 index 000000000..3f5f57522 --- /dev/null +++ b/python/gtsam_py/python/tests/test_StereoVOExample.py @@ -0,0 +1,82 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Stereo VO unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase + + +class TestStereoVOExample(GtsamTestCase): + + def test_StereoVOExample(self): + ## Assumptions + # - For simplicity this example is in the camera's coordinate frame + # - X: right, Y: down, Z: forward + # - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis) + # - x1 is fixed with a constraint, x2 is initialized with noisy values + # - No noise on measurements + + ## Create keys for variables + x1 = symbol(ord('x'),1) + x2 = symbol(ord('x'),2) + l1 = symbol(ord('l'),1) + l2 = symbol(ord('l'),2) + l3 = symbol(ord('l'),3) + + ## Create graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + ## add a constraint on the starting pose + first_pose = gtsam.Pose3() + graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose)) + + ## Create realistic calibration and measurement noise model + # format: fx fy skew cx cy baseline + K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) + stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0])) + + ## Add measurements + # pose 1 + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)) + + #pose 2 + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)) + + ## Create initial estimate for camera poses and landmarks + initialEstimate = gtsam.Values() + initialEstimate.insert(x1, first_pose) + # noisy estimate for pose 2 + initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1))) + expected_l1 = gtsam.Point3( 1, 1, 5) + initialEstimate.insert(l1, expected_l1) + initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5)) + initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5)) + + ## optimize + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimize() + + ## check equality for the first pose and point + pose_x1 = result.atPose3(x1) + self.gtsamAssertEquals(pose_x1, first_pose,1e-4) + + point_l1 = result.atPoint3(l1) + self.gtsamAssertEquals(point_l1, expected_l1,1e-4) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_Values.py b/python/gtsam_py/python/tests/test_Values.py new file mode 100644 index 000000000..20634a21c --- /dev/null +++ b/python/gtsam_py/python/tests/test_Values.py @@ -0,0 +1,86 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Values unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, + Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, + imuBias_ConstantBias) +from gtsam.utils.test_case import GtsamTestCase + + +class TestValues(GtsamTestCase): + + def test_values(self): + values = Values() + E = EssentialMatrix(Rot3(), Unit3()) + tol = 1e-9 + + values.insert(0, Point2(0, 0)) + values.insert(1, Point3(0, 0, 0)) + values.insert(2, Rot2()) + values.insert(3, Pose2()) + values.insert(4, Rot3()) + values.insert(5, Pose3()) + values.insert(6, Cal3_S2()) + values.insert(7, Cal3DS2()) + values.insert(8, Cal3Bundler()) + values.insert(9, E) + values.insert(10, imuBias_ConstantBias()) + + # Special cases for Vectors and Matrices + # Note that gtsam's Eigen Vectors and Matrices requires double-precision + # floating point numbers in column-major (Fortran style) storage order, + # whereas by default, numpy.array is in row-major order and the type is + # in whatever the number input type is, e.g. np.array([1,2,3]) + # will have 'int' type. + # + # The wrapper will automatically fix the type and storage order for you, + # but for performance reasons, it's recommended to specify the correct + # type and storage order. + # for vectors, the order is not important, but dtype still is + vec = np.array([1., 2., 3.]) + values.insert(11, vec) + mat = np.array([[1., 2.], [3., 4.]], order='F') + values.insert(12, mat) + # Test with dtype int and the default order='C' + # This still works as the wrapper converts to the correct type and order for you + # but is nornally not recommended! + mat2 = np.array([[1, 2, ], [3, 5]]) + values.insert(13, mat2) + + self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol) + self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol) + self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol) + self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol) + self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol) + self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol) + self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol) + self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol) + self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol) + self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol) + self.gtsamAssertEquals(values.atimuBias_ConstantBias( + 10), imuBias_ConstantBias(), tol) + + # special cases for Vector and Matrix: + actualVector = values.atVector(11) + np.testing.assert_allclose(vec, actualVector, tol) + actualMatrix = values.atMatrix(12) + np.testing.assert_allclose(mat, actualMatrix, tol) + actualMatrix2 = values.atMatrix(13) + np.testing.assert_allclose(mat2, actualMatrix2, tol) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_VisualISAMExample.py b/python/gtsam_py/python/tests/test_VisualISAMExample.py new file mode 100644 index 000000000..99d7e6160 --- /dev/null +++ b/python/gtsam_py/python/tests/test_VisualISAMExample.py @@ -0,0 +1,57 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +import gtsam.utils.visual_data_generator as generator +import gtsam.utils.visual_isam as visual_isam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase + + +class TestVisualISAMExample(GtsamTestCase): + + def test_VisualISAMExample(self): + # Data Options + options = generator.Options() + options.triangle = False + options.nrCameras = 20 + + # iSAM Options + isamOptions = visual_isam.Options() + isamOptions.hardConstraint = False + isamOptions.pointPriors = False + isamOptions.batchInitialization = True + isamOptions.reorderInterval = 10 + isamOptions.alwaysRelinearize = False + + # Generate data + data, truth = generator.generate_data(options) + + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, options.nrCameras): + isam, result = visual_isam.step(data, isam, result, truth, currentPose) + + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('l'), j)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_dataset.py b/python/gtsam_py/python/tests/test_dataset.py new file mode 100644 index 000000000..60fb9450d --- /dev/null +++ b/python/gtsam_py/python/tests/test_dataset.py @@ -0,0 +1,45 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s +from gtsam.utils.test_case import GtsamTestCase + + +class TestDataset(GtsamTestCase): + """Tests for datasets.h wrapper.""" + + def setUp(self): + """Get some common paths.""" + self.pose3_example_g2o_file = gtsam.findExampleDataFile( + "pose3example.txt") + + def test_readG2o3D(self): + """Test reading directly into factor graph.""" + is3D = True + graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D) + self.assertEqual(graph.size(), 6) + self.assertEqual(initial.size(), 5) + + def test_parse3Dfactors(self): + """Test parsing into data structure.""" + factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) + self.assertEqual(factors.size(), 6) + self.assertIsInstance(factors.at(0), BetweenFactorPose3) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_py/python/tests/test_dsf_map.py b/python/gtsam_py/python/tests/test_dsf_map.py new file mode 100644 index 000000000..73ddcb050 --- /dev/null +++ b/python/gtsam_py/python/tests/test_dsf_map.py @@ -0,0 +1,40 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Disjoint Set Forest. +Author: Frank Dellaert & Varun Agrawal +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestDSFMap(GtsamTestCase): + """Tests for DSFMap.""" + + def test_all(self): + """Test everything in DFSMap.""" + def key(index_pair): + return index_pair.i(), index_pair.j() + + dsf = gtsam.DSFMapIndexPair() + pair1 = gtsam.IndexPair(1, 18) + self.assertEqual(key(dsf.find(pair1)), key(pair1)) + pair2 = gtsam.IndexPair(2, 2) + + # testing the merge feature of dsf + dsf.merge(pair1, pair2) + self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_py/python/tests/test_initialize_pose3.py b/python/gtsam_py/python/tests/test_initialize_pose3.py new file mode 100644 index 000000000..3aa7e3470 --- /dev/null +++ b/python/gtsam_py/python/tests/test_initialize_pose3.py @@ -0,0 +1,89 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for 3D SLAM initialization, using rotation relaxation. +Author: Luca Carlone and Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values +from gtsam.utils.test_case import GtsamTestCase + +x0, x1, x2, x3 = 0, 1, 2, 3 + + +class TestValues(GtsamTestCase): + + def setUp(self): + + model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + + # We consider a small graph: + # symbolic FG + # x2 0 1 + # / | \ 1 2 + # / | \ 2 3 + # x3 | x1 2 0 + # \ | / 0 3 + # \ | / + # x0 + # + p0 = Point3(0, 0, 0) + self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) + p1 = Point3(1, 2, 0) + self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) + p2 = Point3(0, 2, 0) + self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) + p3 = Point3(-1, 1, 0) + self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) + + pose0 = Pose3(self.R0, p0) + pose1 = Pose3(self.R1, p1) + pose2 = Pose3(self.R2, p2) + pose3 = Pose3(self.R3, p3) + + g = NonlinearFactorGraph() + g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) + g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) + g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) + g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) + g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) + g.add(gtsam.PriorFactorPose3(x0, pose0, model)) + self.graph = g + + def test_buildPose3graph(self): + pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + def test_orientations(self): + pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) + + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI + self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6) + + def test_initializePoses(self): + g2oFile = gtsam.findExampleDataFile("pose3example-grid") + is3D = True + inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) + priorModel = gtsam.noiseModel_Unit.Create(6) + inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) + + initial = gtsam.InitializePose3.initialize(inputGraph) + # TODO(frank): very loose !! + self.gtsamAssertEquals(initial, expectedValues, 0.1) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/tests/test_logging_optimizer.py b/python/gtsam_py/python/tests/test_logging_optimizer.py new file mode 100644 index 000000000..2560a72a2 --- /dev/null +++ b/python/gtsam_py/python/tests/test_logging_optimizer.py @@ -0,0 +1,108 @@ +""" +Unit tests for optimization that logs to comet.ml. +Author: Jing Wu and Frank Dellaert +""" +# pylint: disable=invalid-name + +import sys +if sys.version_info.major >= 3: + from io import StringIO +else: + from cStringIO import StringIO + +import unittest +from datetime import datetime + +import gtsam +import numpy as np +from gtsam import Rot3 +from gtsam.utils.test_case import GtsamTestCase + +from gtsam.utils.logging_optimizer import gtsam_optimize + +KEY = 0 +MODEL = gtsam.noiseModel_Unit.Create(3) + + +class TestOptimizeComet(GtsamTestCase): + """Check correct logging to comet.ml.""" + + def setUp(self): + """Set up a small Karcher mean optimization example.""" + # Grabbed from KarcherMeanFactor unit tests. + R = Rot3.Expmap(np.array([0.1, 0, 0])) + rotations = {R, R.inverse()} # mean is the identity + self.expected = Rot3() + + graph = gtsam.NonlinearFactorGraph() + for R in rotations: + graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) + initial = gtsam.Values() + initial.insert(KEY, R) + self.params = gtsam.GaussNewtonParams() + self.optimizer = gtsam.GaussNewtonOptimizer( + graph, initial, self.params) + + self.lmparams = gtsam.LevenbergMarquardtParams() + self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer( + graph, initial, self.lmparams + ) + + # setup output capture + self.capturedOutput = StringIO() + sys.stdout = self.capturedOutput + + def tearDown(self): + """Reset print capture.""" + sys.stdout = sys.__stdout__ + + def test_simple_printing(self): + """Test with a simple hook.""" + + # Provide a hook that just prints + def hook(_, error): + print(error) + + # Only thing we require from optimizer is an iterate method + gtsam_optimize(self.optimizer, self.params, hook) + + # Check that optimizing yields the identity. + actual = self.optimizer.values() + self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + + def test_lm_simple_printing(self): + """Make sure we are properly terminating LM""" + def hook(_, error): + print(error) + + gtsam_optimize(self.lmoptimizer, self.lmparams, hook) + + actual = self.lmoptimizer.values() + self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + + @unittest.skip("Not a test we want run every time, as needs comet.ml account") + def test_comet(self): + """Test with a comet hook.""" + from comet_ml import Experiment + comet = Experiment(project_name="Testing", + auto_output_logging="native") + comet.log_dataset_info(name="Karcher", path="shonan") + comet.add_tag("GaussNewton") + comet.log_parameter("method", "GaussNewton") + time = datetime.now() + comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " " + + str(time.hour)+":"+str(time.minute)+":"+str(time.second)) + + # I want to do some comet thing here + def hook(optimizer, error): + comet.log_metric("Karcher error", + error, optimizer.iterations()) + + gtsam_optimize(self.optimizer, self.params, hook) + comet.end() + + actual = self.optimizer.values() + self.gtsamAssertEquals(actual.atRot3(KEY), self.expected) + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam_py/python/utils/__init__.py b/python/gtsam_py/python/utils/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/gtsam_py/python/utils/circlePose3.py b/python/gtsam_py/python/utils/circlePose3.py new file mode 100644 index 000000000..7012548f4 --- /dev/null +++ b/python/gtsam_py/python/utils/circlePose3.py @@ -0,0 +1,38 @@ +import gtsam +import numpy as np +from math import pi, cos, sin + + +def circlePose3(numPoses=8, radius=1.0, symbolChar=0): + """ + circlePose3 generates a set of poses in a circle. This function + returns those poses inside a gtsam.Values object, with sequential + keys starting from 0. An optional character may be provided, which + will be stored in the msb of each key (i.e. gtsam.Symbol). + + We use aerospace/navlab convention, X forward, Y right, Z down + First pose will be at (R,0,0) + ^y ^ X + | | + z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) + Vehicle at p0 is looking towards y axis (X-axis points towards world y) + """ + + # Force symbolChar to be a single character + if type(symbolChar) is str: + symbolChar = ord(symbolChar[0]) + + values = gtsam.Values() + theta = 0.0 + dtheta = 2 * pi / numPoses + gRo = gtsam.Rot3( + np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) + for i in range(numPoses): + key = gtsam.symbol(symbolChar, i) + gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0) + oRi = gtsam.Rot3.Yaw( + -theta) # negative yaw goes counterclockwise, with Z down ! + gTi = gtsam.Pose3(gRo.compose(oRi), gti) + values.insert(key, gTi) + theta = theta + dtheta + return values diff --git a/python/gtsam_py/python/utils/logging_optimizer.py b/python/gtsam_py/python/utils/logging_optimizer.py new file mode 100644 index 000000000..3d9175951 --- /dev/null +++ b/python/gtsam_py/python/utils/logging_optimizer.py @@ -0,0 +1,52 @@ +""" +Optimization with logging via a hook. +Author: Jing Wu and Frank Dellaert +""" +# pylint: disable=invalid-name + +from gtsam import NonlinearOptimizer, NonlinearOptimizerParams +import gtsam + + +def optimize(optimizer, check_convergence, hook): + """ Given an optimizer and a convergence check, iterate until convergence. + After each iteration, hook(optimizer, error) is called. + After the function, use values and errors to get the result. + Arguments: + optimizer (T): needs an iterate and an error function. + check_convergence: T * float * float -> bool + hook -- hook function to record the error + """ + # the optimizer is created with default values which incur the error below + current_error = optimizer.error() + hook(optimizer, current_error) + + # Iterative loop + while True: + # Do next iteration + optimizer.iterate() + new_error = optimizer.error() + hook(optimizer, new_error) + if check_convergence(optimizer, current_error, new_error): + return + current_error = new_error + + +def gtsam_optimize(optimizer, + params, + hook): + """ Given an optimizer and params, iterate until convergence. + After each iteration, hook(optimizer) is called. + After the function, use values and errors to get the result. + Arguments: + optimizer {NonlinearOptimizer} -- Nonlinear optimizer + params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters + hook -- hook function to record the error + """ + def check_convergence(optimizer, current_error, new_error): + return (optimizer.iterations() >= params.getMaxIterations()) or ( + gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(), + current_error, new_error)) or ( + isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound()) + optimize(optimizer, check_convergence, hook) + return optimizer.values() diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py new file mode 100644 index 000000000..1e976a69e --- /dev/null +++ b/python/gtsam_py/python/utils/plot.py @@ -0,0 +1,369 @@ +"""Various plotting utlities.""" + +import numpy as np +import matplotlib.pyplot as plt +from matplotlib import patches +from mpl_toolkits.mplot3d import Axes3D + +import gtsam + + +def set_axes_equal(fignum): + """ + Make axes of 3D plot have equal scale so that spheres appear as spheres, + cubes as cubes, etc.. This is one possible solution to Matplotlib's + ax.set_aspect('equal') and ax.axis('equal') not working for 3D. + + Args: + fignum (int): An integer representing the figure number for Matplotlib. + """ + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + + limits = np.array([ + ax.get_xlim3d(), + ax.get_ylim3d(), + ax.get_zlim3d(), + ]) + + origin = np.mean(limits, axis=1) + radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) + + ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) + ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) + ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) + + +def ellipsoid(xc, yc, zc, rx, ry, rz, n): + """ + Numpy equivalent of Matlab's ellipsoid function. + + Args: + xc (double): Center of ellipsoid in X-axis. + yc (double): Center of ellipsoid in Y-axis. + zc (double): Center of ellipsoid in Z-axis. + rx (double): Radius of ellipsoid in X-axis. + ry (double): Radius of ellipsoid in Y-axis. + rz (double): Radius of ellipsoid in Z-axis. + n (int): The granularity of the ellipsoid plotted. + + Returns: + tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot. + """ + u = np.linspace(0, 2*np.pi, n+1) + v = np.linspace(0, np.pi, n+1) + x = -rx * np.outer(np.cos(u), np.sin(v)).T + y = -ry * np.outer(np.sin(u), np.sin(v)).T + z = -rz * np.outer(np.ones_like(u), np.cos(v)).T + + return x, y, z + + +def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): + """ + Plots a Gaussian as an uncertainty ellipse + + Based on Maybeck Vol 1, page 366 + k=2.296 corresponds to 1 std, 68.26% of all probability + k=11.82 corresponds to 3 std, 99.74% of all probability + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + origin (gtsam.Point3): The origin in the world frame. + P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. + scale (float): Scaling factor of the radii of the covariance ellipse. + n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. + alpha (float): Transparency value for the plotted surface in the range [0, 1]. + """ + k = 11.82 + U, S, _ = np.linalg.svd(P) + + radii = k * np.sqrt(S) + radii = radii * scale + rx, ry, rz = radii + + # generate data for "unrotated" ellipsoid + xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n) + + # rotate data with orientation matrix U and center c + data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \ + np.kron(U[:, 2:3], zc) + n = data.shape[1] + x = data[0:n, :] + origin[0] + y = data[n:2*n, :] + origin[1] + z = data[2*n:, :] + origin[2] + + axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') + + +def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): + """ + Plot a 2D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ + # 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-') + + if covariance is not None: + pPp = covariance[0:2, 0:2] + gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) + + w, v = np.linalg.eig(gPp) + + # k = 2.296 + k = 5.0 + + angle = np.arctan2(v[1, 0], v[0, 0]) + e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), + np.rad2deg(angle), fill=False) + axes.add_patch(e1) + + +def plot_pose2(fignum, pose, axis_length=0.1, covariance=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a 2D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + """ + # get figure object + fig = plt.figure(fignum) + axes = fig.gca() + plot_pose2_on_axes(axes, pose, axis_length=axis_length, + covariance=covariance) + + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig + + +def plot_point3_on_axes(axes, point, linespec, P=None): + """ + Plot a 3D point on given axis `axes` with given `linespec`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ + axes.plot([point.x()], [point.y()], [point.z()], linespec) + if P is not None: + plot_covariance_ellipse_3d(axes, point.vector(), P) + + +def plot_point3(fignum, point, linespec, P=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a 3D point on given figure with given `linespec`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + + Returns: + fig: The matplotlib figure. + + """ + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plot_point3_on_axes(axes, point, linespec, P) + + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig + + +def plot_3d_points(fignum, values, linespec="g*", marginals=None, + title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + 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. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dictionary consisting of points to be plotted. + linespec (string): String representing formatting options for Matplotlib. + marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + title (string): The title of the plot. + axis_labels (iterable[string]): List of axis labels to set. + """ + + keys = values.keys() + + # Plot points and covariance matrices + for i in range(keys.size()): + try: + key = keys.at(i) + point = values.atPoint3(key) + if marginals is not None: + covariance = marginals.marginalCovariance(key) + else: + covariance = None + + fig = plot_point3(fignum, point, linespec, covariance, + axis_labels=axis_labels) + + except RuntimeError: + continue + # I guess it's not a Point3 + + fig.suptitle(title) + fig.canvas.set_window_title(title.lower()) + + +def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): + """ + Plot a 3D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + origin = pose.translation().vector() + + # 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 P is not None: + # covariance matrix in pose coordinate frame + pPp = P[3:6, 3:6] + # convert the covariance matrix to global coordinate frame + gPp = gRp @ pPp @ gRp.T + plot_covariance_ellipse_3d(axes, origin, gPp) + + +def plot_pose3(fignum, pose, axis_length=0.1, P=None, + axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a 3D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose3): 3D pose to be plotted. + linespec (string): String representing formatting options for Matplotlib. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels (iterable[string]): List of axis labels to set. + + Returns: + fig: The matplotlib figure. + """ + # get figure object + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + plot_pose3_on_axes(axes, pose, P=P, + axis_length=axis_length) + + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + axes.set_zlabel(axis_labels[2]) + + return fig + + +def plot_trajectory(fignum, values, scale=1, marginals=None, + title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')): + """ + Plot a complete 3D trajectory using poses in `values`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dict containing the poses. + scale (float): Value to scale the poses by. + marginals (gtsam.Marginals): Marginalized probability values of the estimation. + Used to plot uncertainty bounds. + title (string): The title of the plot. + axis_labels (iterable[string]): List of axis labels to set. + """ + pose3Values = gtsam.utilities_allPose3s(values) + keys = gtsam.KeyVector(pose3Values.keys()) + lastIndex = None + + for i in range(keys.size()): + key = keys.at(i) + try: + pose = pose3Values.atPose3(key) + except: + print("Warning: no Pose3 at key: {0}".format(key)) + + if lastIndex is not None: + lastKey = keys.at(lastIndex) + try: + lastPose = pose3Values.atPose3(lastKey) + except: + print("Warning: no Pose3 at key: {0}".format(lastKey)) + pass + + if marginals: + covariance = marginals.marginalCovariance(lastKey) + else: + covariance = None + + fig = plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale, axis_labels=axis_labels) + + lastIndex = i + + # Draw final pose + if lastIndex is not None: + lastKey = keys.at(lastIndex) + try: + lastPose = pose3Values.atPose3(lastKey) + if marginals: + covariance = marginals.marginalCovariance(lastKey) + else: + covariance = None + + fig = plot_pose3(fignum, lastPose, P=covariance, + axis_length=scale, axis_labels=axis_labels) + + except: + pass + + fig.suptitle(title) + fig.canvas.set_window_title(title.lower()) diff --git a/python/gtsam_py/python/utils/test_case.py b/python/gtsam_py/python/utils/test_case.py new file mode 100644 index 000000000..7df1e6ee9 --- /dev/null +++ b/python/gtsam_py/python/utils/test_case.py @@ -0,0 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +TestCase class with GTSAM assert utils. +Author: Frank Dellaert +""" +import unittest + + +class GtsamTestCase(unittest.TestCase): + """TestCase class with GTSAM assert utils.""" + + def gtsamAssertEquals(self, actual, expected, tol=1e-9): + """ AssertEqual function that prints out actual and expected if not equal. + Usage: + self.gtsamAssertEqual(actual,expected) + Keyword Arguments: + tol {float} -- tolerance passed to 'equals', default 1e-9 + """ + equal = actual.equals(expected, tol) + if not equal: + raise self.failureException( + "Values are not equal:\n{}!={}".format(actual, expected)) diff --git a/python/gtsam_py/python/utils/visual_data_generator.py b/python/gtsam_py/python/utils/visual_data_generator.py new file mode 100644 index 000000000..f04588e70 --- /dev/null +++ b/python/gtsam_py/python/utils/visual_data_generator.py @@ -0,0 +1,118 @@ +from __future__ import print_function + +import numpy as np + +import gtsam +from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3 + + +class Options: + """ + Options to generate test scenario + """ + + def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): + """ + Options to generate test scenario + @param triangle: generate a triangle scene with 3 points if True, otherwise + a cube with 8 points + @param nrCameras: number of cameras to generate + @param K: camera calibration object + """ + self.triangle = triangle + self.nrCameras = nrCameras + + +class GroundTruth: + """ + Object holding generated ground-truth data + """ + + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + self.K = K + self.cameras = [Pose3()] * nrCameras + self.points = [Point3()] * nrPoints + + def print_(self, s=""): + print(s) + print("K = ", self.K) + print("Cameras: ", len(self.cameras)) + for camera in self.cameras: + print("\t", camera) + print("Points: ", len(self.points)) + for point in self.points: + print("\t", point) + pass + + +class Data: + """ + Object holding generated measurement data + """ + + class NoiseModels: + pass + + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + self.K = K + self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras] + self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] + self.odometry = [Pose3()] * nrCameras + + # Set Noise parameters + self.noiseModels = Data.NoiseModels() + self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) + # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( + # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) + self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) + self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) + + +def generate_data(options): + """ Generate ground-truth and measurement data. """ + + K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + nrPoints = 3 if options.triangle else 8 + + truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) + data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints) + + # Generate simulated data + if options.triangle: # Create a triangle target, just 3 points on a plane + r = 10 + for j in range(len(truth.points)): + theta = j * 2 * np.pi / nrPoints + truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0) + else: # 3D landmarks as vertices of a cube + truth.points = [ + Point3(10, 10, 10), Point3(-10, 10, 10), + Point3(-10, -10, 10), Point3(10, -10, 10), + Point3(10, 10, -10), Point3(-10, 10, -10), + Point3(-10, -10, -10), Point3(10, -10, -10) + ] + + # Create camera cameras on a circle around the triangle + height = 10 + r = 40 + for i in range(options.nrCameras): + theta = i * 2 * np.pi / options.nrCameras + t = Point3(r * np.cos(theta), r * np.sin(theta), height) + truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, + Point3(), + Point3(0, 0, 1), + truth.K) + # Create measurements + for j in range(nrPoints): + # All landmarks seen in every frame + data.Z[i][j] = truth.cameras[i].project(truth.points[j]) + data.J[i][j] = j + + # Calculate odometry between cameras + for i in range(1, options.nrCameras): + data.odometry[i] = truth.cameras[i - 1].pose().between( + truth.cameras[i].pose()) + + return data, truth diff --git a/python/gtsam_py/python/utils/visual_isam.py b/python/gtsam_py/python/utils/visual_isam.py new file mode 100644 index 000000000..b0ebe68c3 --- /dev/null +++ b/python/gtsam_py/python/utils/visual_isam.py @@ -0,0 +1,131 @@ +import gtsam +from gtsam import symbol + + +class Options: + """ Options for visual isam example. """ + + def __init__(self): + self.hardConstraint = False + self.pointPriors = False + self.batchInitialization = True + self.reorderInterval = 10 + self.alwaysRelinearize = False + + +def initialize(data, truth, options): + # Initialize iSAM + params = gtsam.ISAM2Params() + if options.alwaysRelinearize: + params.setRelinearizeSkip(1) + isam = gtsam.ISAM2(params=params) + + # Add constraints/priors + # TODO: should not be from ground truth! + newFactors = gtsam.NonlinearFactorGraph() + initialEstimates = gtsam.Values() + for i in range(2): + ii = symbol(ord('x'), i) + if i == 0: + if options.hardConstraint: # add hard constraint + newFactors.add( + gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose())) + else: + newFactors.add( + gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(), + data.noiseModels.posePrior)) + initialEstimates.insert(ii, truth.cameras[i].pose()) + + nextPoseIndex = 2 + + # Add visual measurement factors from two first poses and initialize + # observed landmarks + for i in range(2): + ii = symbol(ord('x'), i) + for k in range(len(data.Z[i])): + j = data.J[i][k] + jj = symbol(ord('l'), j) + newFactors.add( + gtsam.GenericProjectionFactorCal3_S2(data.Z[i][ + k], data.noiseModels.measurement, ii, jj, data.K)) + # TODO: initial estimates should not be from ground truth! + if not initialEstimates.exists(jj): + initialEstimates.insert(jj, truth.points[j]) + if options.pointPriors: # add point priors + newFactors.add( + gtsam.PriorFactorPoint3(jj, truth.points[j], + data.noiseModels.pointPrior)) + + # Add odometry between frames 0 and 1 + newFactors.add( + gtsam.BetweenFactorPose3( + symbol(ord('x'), 0), + symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry)) + + # Update ISAM + if options.batchInitialization: # Do a full optimize for first two poses + batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors, + initialEstimates) + fullyOptimized = batchOptimizer.optimize() + isam.update(newFactors, fullyOptimized) + else: + isam.update(newFactors, initialEstimates) + + # figure(1)tic + # t=toc plot(frame_i,t,'r.') tic + result = isam.calculateEstimate() + # t=toc plot(frame_i,t,'g.') + + return isam, result, nextPoseIndex + + +def step(data, isam, result, truth, currPoseIndex): + ''' + Do one step isam update + @param[in] data: measurement data (odometry and visual measurements and their noiseModels) + @param[in/out] isam: current isam object, will be updated + @param[in/out] result: current result object, will be updated + @param[in] truth: ground truth data, used to initialize new variables + @param[currPoseIndex]: index of the current pose + ''' + # iSAM expects us to give it a new set of factors + # along with initial estimates for any new variables introduced. + newFactors = gtsam.NonlinearFactorGraph() + initialEstimates = gtsam.Values() + + # Add odometry + prevPoseIndex = currPoseIndex - 1 + odometry = data.odometry[prevPoseIndex] + newFactors.add( + gtsam.BetweenFactorPose3( + symbol(ord('x'), prevPoseIndex), + symbol(ord('x'), currPoseIndex), odometry, + data.noiseModels.odometry)) + + # Add visual measurement factors and initializations as necessary + for k in range(len(data.Z[currPoseIndex])): + zij = data.Z[currPoseIndex][k] + j = data.J[currPoseIndex][k] + jj = symbol(ord('l'), j) + newFactors.add( + gtsam.GenericProjectionFactorCal3_S2( + zij, data.noiseModels.measurement, + symbol(ord('x'), currPoseIndex), jj, data.K)) + # TODO: initialize with something other than truth + if not result.exists(jj) and not initialEstimates.exists(jj): + lmInit = truth.points[j] + initialEstimates.insert(jj, lmInit) + + # Initial estimates for the new pose. + prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex)) + initialEstimates.insert( + symbol(ord('x'), currPoseIndex), prevPose.compose(odometry)) + + # Update ISAM + # figure(1)tic + isam.update(newFactors, initialEstimates) + # t=toc plot(frame_i,t,'r.') tic + newResult = isam.calculateEstimate() + # t=toc plot(frame_i,t,'g.') + + return isam, newResult