From d3591dac0b4fa96d98db4859863c9410a97b3e5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jun 2020 15:07:54 -0500 Subject: [PATCH 01/48] fix deprecation warnings from Matplotlib --- .../gtsam/examples/PreintegrationExample.py | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/cython/gtsam/examples/PreintegrationExample.py b/cython/gtsam/examples/PreintegrationExample.py index 76520b355..68b1c8cc0 100644 --- a/cython/gtsam/examples/PreintegrationExample.py +++ b/cython/gtsam/examples/PreintegrationExample.py @@ -73,38 +73,41 @@ class PreintegrationExample(object): self.runner = gtsam.ScenarioRunner( self.scenario, self.params, self.dt, self.actualBias) + fig, self.axes = plt.subplots(4, 3) + fig.set_tight_layout(True) + 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) + ax = self.axes[0][i] + ax.scatter(t, omega_b[i], color='k', marker='.') + ax.scatter(t, measuredOmega[i], color=color, marker='.') + ax.set_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) + ax = self.axes[1][i] + ax.scatter(t, acceleration_n[i], color=color, marker='.') + ax.set_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) + ax = self.axes[2][i] + ax.scatter(t, acceleration_b[i], color=color, marker='.') + ax.set_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) + ax = self.axes[3][i] + ax.scatter(t, actual[i], color='k', marker='.') + ax.scatter(t, measuredAcc[i], color=color, marker='.') + ax.set_xlabel('specific force ' + label) def plotGroundTruthPose(self, t): # plot ground truth pose, as well as prediction from integrated IMU measurements From 3db8862480abf919f208065e1c2976c29f19b52c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jun 2020 15:09:11 -0500 Subject: [PATCH 02/48] reduce for loops, add titles to plots, better space subplots for IMU data --- cython/gtsam/examples/ImuFactorExample.py | 36 +++++++++++------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 0e01766e7..6cb06dcd0 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -69,32 +69,19 @@ class ImuFactorExample(PreintegrationExample): 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) + initial.insert(X(i), actual_state_i.pose()) + initial.insert(V(i), actual_state_i.velocity()) + 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) @@ -102,6 +89,7 @@ class ImuFactorExample(PreintegrationExample): # Plot every second if k % int(1 / self.dt) == 0: self.plotGroundTruthPose(t) + plt.title("Ground Truth Trajectory") # create IMU factor every second if (k + 1) % int(1 / self.dt) == 0: @@ -112,6 +100,17 @@ class ImuFactorExample(PreintegrationExample): print(factor) print(pim.predict(actual_state_i, self.actualBias)) pim.resetIntegration() + + rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) + translationNoise = gtsam.Point3(np.random.randn(3)*0.1) + poseNoise = gtsam.Pose3(rotationNoise, translationNoise) + + actual_state_i = gtsam.NavState( + actual_state_i.pose().compose(poseNoise), + actual_state_i.velocity() + np.random.randn(3)*0.1) + + initial.insert(X(i+1), actual_state_i.pose()) + initial.insert(V(i+1), actual_state_i.velocity()) actual_state_i = self.scenario.navState(t + self.dt) i += 1 @@ -138,10 +137,11 @@ class ImuFactorExample(PreintegrationExample): i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG, pose_i, 0.1) + plot_pose3(POSES_FIG+1, pose_i, 0.1) i += 1 + plt.title("Estimated Trajectory") - gtsam.utils.plot.set_axes_equal(POSES_FIG) + gtsam.utils.plot.set_axes_equal(POSES_FIG+1) print(result.atimuBias_ConstantBias(BIAS_KEY)) From 33a1628e84e5d9dbcc5bf88cd12156a74d6f2eae Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jun 2020 17:30:30 -0500 Subject: [PATCH 03/48] vastly improved the basic ImuFactorExample script --- cython/gtsam/examples/ImuFactorExample.py | 49 ++++++++++++++--------- 1 file changed, 31 insertions(+), 18 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 6cb06dcd0..d323c1b17 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -12,6 +12,7 @@ Author: Frank Dellaert, Varun Agrawal from __future__ import print_function +import argparse import math import gtsam @@ -33,24 +34,27 @@ np.set_printoptions(precision=3, suppress=True) class ImuFactorExample(PreintegrationExample): - def __init__(self): + def __init__(self, twist_scenario="sick_twist"): 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) + twist_scenarios = dict( + 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) + super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], + bias, dt) def addPrior(self, i, graph): state = self.scenario.navState(i) @@ -59,13 +63,12 @@ class ImuFactorExample(PreintegrationExample): graph.push_back(gtsam.PriorFactorVector( V(i), state.velocity(), self.velNoise)) - def run(self): + def run(self, T=12, verbose=True): 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) @@ -86,32 +89,34 @@ class ImuFactorExample(PreintegrationExample): if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) - # Plot every second - if k % int(1 / self.dt) == 0: + if (k+1) % int(1 / self.dt) == 0: + # Plot every second self.plotGroundTruthPose(t) plt.title("Ground Truth Trajectory") - # 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) + # create IMU factor every second + factor = gtsam.ImuFactor(X(i), V(i), + X(i + 1), V(i + 1), + BIAS_KEY, pim) graph.push_back(factor) - if True: + + if verbose: print(factor) print(pim.predict(actual_state_i, self.actualBias)) + pim.resetIntegration() rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) translationNoise = gtsam.Point3(np.random.randn(3)*0.1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) + actual_state_i = self.scenario.navState(t + self.dt) actual_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3)*0.1) initial.insert(X(i+1), actual_state_i.pose()) initial.insert(V(i+1), actual_state_i.velocity()) - actual_state_i = self.scenario.navState(t + self.dt) i += 1 # add priors on beginning and end @@ -150,4 +155,12 @@ class ImuFactorExample(PreintegrationExample): if __name__ == '__main__': - ImuFactorExample().run() + parser = argparse.ArgumentParser("ImuFactorExample.py") + parser.add_argument("--twist_scenario", + default="sick_twist", + choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) + parser.add_argument("--time", "-T", default=12, type=int, help="Total time in seconds") + parser.add_argument("--verbose", default=False, action='store_true') + args = parser.parse_args() + + ImuFactorExample().run(args.time, args.verbose) From 52a8dde6a7f041c7b780b41865c7c95225e875a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 17 Jun 2020 13:47:43 -0500 Subject: [PATCH 04/48] updated filename in brief --- examples/ImuFactorsExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index a4707ea46..668b48934 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file imuFactorsExample + * @file ImuFactorsExample * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor * navigation code. * @author Garrett (ghemann@gmail.com), Luca Carlone From 17bf29d4b05b7baf777861719b6e5f5dd445d561 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Jun 2020 20:24:54 -0500 Subject: [PATCH 05/48] improved result printing and use of flags for ImuFactorExample.py --- cython/gtsam/examples/ImuFactorExample.py | 48 +++++++++++++---------- 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index d323c1b17..bb0424c85 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -63,7 +63,7 @@ class ImuFactorExample(PreintegrationExample): graph.push_back(gtsam.PriorFactorVector( V(i), state.velocity(), self.velNoise)) - def run(self, T=12, verbose=True): + def run(self, T=12, compute_covariances=False, verbose=True): graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements @@ -75,9 +75,9 @@ class ImuFactorExample(PreintegrationExample): # simulate the loop i = 0 # state index - actual_state_i = self.scenario.navState(0) - initial.insert(X(i), actual_state_i.pose()) - initial.insert(V(i), actual_state_i.velocity()) + initial_state_i = self.scenario.navState(0) + initial.insert(X(i), initial_state_i.pose()) + initial.insert(V(i), initial_state_i.velocity()) for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM @@ -111,17 +111,20 @@ class ImuFactorExample(PreintegrationExample): poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) - actual_state_i = gtsam.NavState( + print("Actual state at {0}:\n{1}".format(t+self.dt, actual_state_i)) + + noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3)*0.1) - initial.insert(X(i+1), actual_state_i.pose()) - initial.insert(V(i+1), actual_state_i.velocity()) + initial.insert(X(i+1), noisy_state_i.pose()) + initial.insert(V(i+1), noisy_state_i.velocity()) i += 1 - # add priors on beginning and end + # add prior on beginning self.addPrior(0, graph) - self.addPrior(num_poses - 1, graph) + # add prior on end + # self.addPrior(num_poses - 1, graph) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() @@ -129,14 +132,17 @@ class ImuFactorExample(PreintegrationExample): 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)))) + result.print_("") + + if compute_covariances: + # 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 @@ -148,7 +154,7 @@ class ImuFactorExample(PreintegrationExample): gtsam.utils.plot.set_axes_equal(POSES_FIG+1) - print(result.atimuBias_ConstantBias(BIAS_KEY)) + print("Bias Values", result.atimuBias_ConstantBias(BIAS_KEY)) plt.ioff() plt.show() @@ -159,8 +165,10 @@ if __name__ == '__main__': parser.add_argument("--twist_scenario", default="sick_twist", choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist")) - parser.add_argument("--time", "-T", default=12, type=int, help="Total time in seconds") + parser.add_argument("--time", "-T", default=12, + type=int, help="Total time in seconds") + parser.add_argument("--compute_covariances", default=False, action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() - ImuFactorExample().run(args.time, args.verbose) + ImuFactorExample(args.twist_scenario).run(args.time, args.compute_covariances, args.verbose) From bfbb6cb28d81dd2fb18ded67ca4e805f0fbb291c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Jun 2020 16:32:46 -0500 Subject: [PATCH 06/48] renamed ImuFactorExample2 to ImuFactorISAM2Example --- ...orExample2.py => ImuFactorISAM2Example.py} | 122 +++++++++--------- 1 file changed, 60 insertions(+), 62 deletions(-) rename cython/gtsam/examples/{ImuFactorExample2.py => ImuFactorISAM2Example.py} (69%) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorISAM2Example.py similarity index 69% rename from cython/gtsam/examples/ImuFactorExample2.py rename to cython/gtsam/examples/ImuFactorISAM2Example.py index 0d98f08fe..3d3138bf5 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorISAM2Example.py @@ -1,6 +1,6 @@ """ iSAM2 example with ImuFactor. -Author: Robert Truax (C++), Frank Dellaert (Python) +Author: Frank Dellaert, Varun Agrawal """ # pylint: disable=invalid-name, E1101 @@ -8,10 +8,11 @@ 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 mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 + +import gtsam from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, @@ -20,7 +21,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, 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 +from gtsam.utils import plot def vector3(x, y, z): @@ -28,46 +29,46 @@ def vector3(x, y, z): 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)) +kGravity = vector3(0, 0, -g) -BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) -DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), - Point3(0.05, -0.10, 0.20)) + +def preintegration_parameters(): + # IMU preintegration parameters + # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + 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)) + + return PARAMS, BIAS_COVARIANCE, DELTA + + +def get_camera(radius): + up = Point3(0, 0, 1) + target = Point3(0, 0, 0) + position = Point3(radius, 0, 0) + camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) + return camera + + +def get_scenario(radius, pose_0, angular_velocity, delta_t): + """Create the set of ground-truth landmarks and poses""" + + + 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) + + return scenario def IMU_example(): @@ -75,23 +76,17 @@ def IMU_example(): # 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()) + camera = get_camera(radius) 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 = math.radians(180) # rad/sec + scenario = get_scenario(radius, pose_0, angular_velocity, delta_t) - 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) + PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters() # Create a factor graph - newgraph = NonlinearFactorGraph() + graph = NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = ISAM2() @@ -104,21 +99,21 @@ def IMU_example(): # 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)) + graph.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) + graph.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) + graph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) @@ -140,33 +135,36 @@ def IMU_example(): biasKey += 1 factor = BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) - newgraph.add(factor) + graph.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) + measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, kGravity) 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) + graph.add(imufac) # insert new velocity, which is wrong initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution - isam.update(newgraph, initialEstimate) + isam.update(graph, initialEstimate) result = isam.calculateEstimate() - ISAM2_plot(result) + plot.plot_incremental_trajectory(0, result, + start=i, scale=3, time_interval=0.01) # reset - newgraph = NonlinearFactorGraph() + graph = NonlinearFactorGraph() initialEstimate.clear() + plt.show() + if __name__ == '__main__': IMU_example() From 89bdebf5ca96233b2d82d14741a1c1e58d53b1c3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Jun 2020 16:33:09 -0500 Subject: [PATCH 07/48] added function to plot trajectory incrementally --- cython/gtsam/utils/plot.py | 41 +++++++++++++++++++++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index b67444fc1..cbc7c227a 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -268,7 +268,8 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None): axis_length=axis_length) -def plot_trajectory(fignum, values, scale=1, marginals=None): +def plot_trajectory(fignum, values, scale=1, marginals=None, + animation_interval=0.0): """ Plot a complete 3D trajectory using poses in `values`. @@ -323,3 +324,41 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): except: pass + + +def plot_incremental_trajectory(fignum, values, start=0, + scale=1, marginals=None, + time_interval=0.0): + """ + Incrementally 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. + start (int): Starting index to start plotting from. + scale (float): Value to scale the poses by. + marginals (gtsam.Marginals): Marginalized probability values of the estimation. + Used to plot uncertainty bounds. + time_interval (float): Time in seconds to pause between each rendering. + Used to create animation effect. + """ + fig = plt.figure(fignum) + axes = fig.gca(projection='3d') + + pose3Values = gtsam.utilities_allPose3s(values) + keys = gtsam.KeyVector(pose3Values.keys()) + + for i in range(start, keys.size()): + key = keys.at(i) + if values.exists(key): + pose_i = values.atPose3(keys.at(i)) + plot_pose3(fignum, pose_i, scale) + + # Update the plot space to encompass all plotted points + axes.autoscale() + + # Set the 3 axes equal + set_axes_equal(fignum) + + # Pause for a fixed amount of seconds + plt.pause(time_interval) From 6fdcddbaa7678620d46fbade21850f4e74e312bb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Jun 2020 16:33:28 -0500 Subject: [PATCH 08/48] improvements to ImuFactorExample --- cython/gtsam/examples/ImuFactorExample.py | 9 +++++---- cython/gtsam/examples/PreintegrationExample.py | 9 ++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index bb0424c85..289a0ccec 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample): # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - num_poses = T + 1 # assumes 1 factor per second + num_poses = T # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) @@ -91,7 +91,7 @@ class ImuFactorExample(PreintegrationExample): if (k+1) % int(1 / self.dt) == 0: # Plot every second - self.plotGroundTruthPose(t) + self.plotGroundTruthPose(t, scale=1) plt.title("Ground Truth Trajectory") # create IMU factor every second @@ -107,7 +107,7 @@ class ImuFactorExample(PreintegrationExample): pim.resetIntegration() rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1) - translationNoise = gtsam.Point3(np.random.randn(3)*0.1) + translationNoise = gtsam.Point3(np.random.randn(3)*1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) @@ -132,6 +132,7 @@ class ImuFactorExample(PreintegrationExample): optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() + initial.print_("Initial values:") result.print_("") if compute_covariances: @@ -148,7 +149,7 @@ class ImuFactorExample(PreintegrationExample): i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG+1, pose_i, 0.1) + plot_pose3(POSES_FIG+1, pose_i, 1) i += 1 plt.title("Estimated Trajectory") diff --git a/cython/gtsam/examples/PreintegrationExample.py b/cython/gtsam/examples/PreintegrationExample.py index 68b1c8cc0..958221ac9 100644 --- a/cython/gtsam/examples/PreintegrationExample.py +++ b/cython/gtsam/examples/PreintegrationExample.py @@ -109,10 +109,10 @@ class PreintegrationExample(object): ax.scatter(t, measuredAcc[i], color=color, marker='.') ax.set_xlabel('specific force ' + label) - def plotGroundTruthPose(self, t): + def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): # plot ground truth pose, as well as prediction from integrated IMU measurements actualPose = self.scenario.pose(t) - plot_pose3(POSES_FIG, actualPose, 0.3) + plot_pose3(POSES_FIG, actualPose, scale) t = actualPose.translation() self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) ax = plt.gca() @@ -120,11 +120,10 @@ class PreintegrationExample(object): ax.set_ylim3d(-self.maxDim, self.maxDim) ax.set_zlim3d(-self.maxDim, self.maxDim) - plt.pause(0.01) + plt.pause(time_interval) - def run(self): + def run(self, T=12): # 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) From 561218ae822039fb48d1911c7344599208de3747 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Jun 2020 16:34:05 -0500 Subject: [PATCH 09/48] updated file docstring for ImuFactorExample2.cpp --- examples/ImuFactorExample2.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index cb3650421..0031acbe8 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,3 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file ImuFactorExample2 + * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor with ISAM2. + * @author Robert Truax + */ #include #include From ebd4db2380ce9d9c14e8ce3ccbbc08e37657fcc2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jul 2020 13:03:17 -0500 Subject: [PATCH 10/48] small improvements to the ImuFactorExample.py --- cython/gtsam/examples/ImuFactorExample.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py index 289a0ccec..06742fcd1 100644 --- a/cython/gtsam/examples/ImuFactorExample.py +++ b/cython/gtsam/examples/ImuFactorExample.py @@ -79,6 +79,9 @@ class ImuFactorExample(PreintegrationExample): initial.insert(X(i), initial_state_i.pose()) initial.insert(V(i), initial_state_i.velocity()) + # add prior on beginning + self.addPrior(0, graph) + for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) @@ -121,19 +124,18 @@ class ImuFactorExample(PreintegrationExample): initial.insert(V(i+1), noisy_state_i.velocity()) i += 1 - # add prior on beginning - self.addPrior(0, graph) # add prior on end # self.addPrior(num_poses - 1, graph) + initial.print_("Initial values:") + # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() - initial.print_("Initial values:") - result.print_("") + result.print_("Optimized values:") if compute_covariances: # Calculate and print marginal covariances From aae9c19d0fa7dc152b0818511a0a7d11e96c53bc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Aug 2020 00:25:42 -0500 Subject: [PATCH 11/48] example using CombinedImuFactor --- examples/CombinedImuFactorsExample.cpp | 287 +++++++++++++++++++++++++ 1 file changed, 287 insertions(+) create mode 100644 examples/CombinedImuFactorsExample.cpp diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp new file mode 100644 index 000000000..f04f2ddd4 --- /dev/null +++ b/examples/CombinedImuFactorsExample.cpp @@ -0,0 +1,287 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file CombinedImuFactorsExample + * @brief Test example for using GTSAM ImuCombinedFactor + * navigation code. + * @author Varun Agrawal + */ + +/** + * Example of use of the CombinedImuFactor in + * conjunction with GPS + * - we read IMU and GPS data from a CSV file, with the following format: + * A row starting with "i" is the first initial position formatted with + * N, E, D, qx, qY, qZ, qW, velN, velE, velD + * A row starting with "0" is an imu measurement + * linAccN, linAccE, linAccD, angVelN, angVelE, angVelD + * A row starting with "1" is a gps correction formatted with + * N, E, D, qX, qY, qZ, qW + * Note that for GPS correction, we're only using the position not the + * rotation. The rotation is provided in the file for ground truth comparison. + * + * Usage: ./CombinedImuFactorsExample [data_csv_path] [-c] + * optional arguments: + * data_csv_path path to the CSV file with the IMU data. + * -c use CombinedImuFactor + */ + +// GTSAM related includes. +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace gtsam; +using namespace std; + +using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) + +Vector10 readInitialState(ifstream& file) { + string value; + // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) + Vector10 initial_state; + getline(file, value, ','); // i + for (int i = 0; i < 9; i++) { + getline(file, value, ','); + initial_state(i) = atof(value.c_str()); + } + getline(file, value, '\n'); + initial_state(9) = atof(value.c_str()); + + return initial_state; +} + +boost::shared_ptr imuParams() { + // We use the sensor specs to build the noise model for the IMU factor. + double accel_noise_sigma = 0.0003924; + double gyro_noise_sigma = 0.000205689024915; + double accel_bias_rw_sigma = 0.004905; + double gyro_bias_rw_sigma = 0.000001454441043; + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration + + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + // PreintegrationBase params: + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + // should be using 2nd order integration + // PreintegratedRotation params: + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + // PreintegrationCombinedMeasurements params: + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccOmegaInt = bias_acc_omega_int; + + return p; +} + +int main(int argc, char* argv[]) { + string data_filename, output_filename; + if (argc == 3) { + data_filename = argv[1]; + output_filename = argv[2]; + } else if (argc == 2) { + data_filename = argv[1]; + output_filename = "imuFactorExampleResults.csv"; + } else { + printf("using default CSV file\n"); + data_filename = findExampleDataFile("imuAndGPSdata.csv"); + output_filename = "imuFactorExampleResults.csv"; + } + + // Set up output file for plotting errors + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx," + "gt_qy,gt_qz,gt_qw\n"); + + // Begin parsing the CSV file. Input the first line for initialization. + // From there, we'll iterate through the file and we'll preintegrate the IMU + // or add in the GPS given the input. + ifstream file(data_filename.c_str()); + + Vector10 initial_state = readInitialState(file); + cout << "initial state:\n" << initial_state.transpose() << "\n\n"; + + // Assemble initial quaternion through GTSAM constructor + // ::Quaternion(w,x,y,z); + Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), + initial_state(4), initial_state(5)); + Point3 prior_point(initial_state.head<3>()); + Pose3 prior_pose(prior_rotation, prior_point); + Vector3 prior_velocity(initial_state.tail<3>()); + + imuBias::ConstantBias prior_imu_bias; // assume zero initial bias + + int index = 0; + + Values initial_values; + + // insert pose at initialization + initial_values.insert(X(index), prior_pose); + initial_values.insert(V(index), prior_velocity); + initial_values.insert(B(index), prior_imu_bias); + + // Assemble prior noise model and add it the graph.` + auto pose_noise_model = noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5) + .finished()); // rad,rad,rad,m, m, m + auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s + auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3); + + // Add all prior factors (pose, velocity, bias) to the graph. + NonlinearFactorGraph graph; + graph.addPrior(X(index), prior_pose, pose_noise_model); + graph.addPrior(V(index), prior_velocity, velocity_noise_model); + graph.addPrior(B(index), prior_imu_bias, + bias_noise_model); + + auto p = imuParams(); + + std::shared_ptr preintegrated = + std::make_shared(p, prior_imu_bias); + + assert(preintegrated); + + // Store previous state for imu integration and latest predicted outcome. + NavState prev_state(prior_pose, prior_velocity); + NavState prop_state = prev_state; + imuBias::ConstantBias prev_bias = prior_imu_bias; + + // Keep track of total error over the entire run as simple performance metric. + double current_position_error = 0.0, current_orientation_error = 0.0; + + double output_time = 0.0; + double dt = 0.005; // The real system has noise, but here, results are nearly + // exactly the same, so keeping this for simplicity. + + // All priors have been set up, now iterate through the data file. + while (file.good()) { + // Parse out first value + string value; + getline(file, value, ','); + int type = atoi(value.c_str()); + + if (type == 0) { // IMU measurement + Vector6 imu; + for (int i = 0; i < 5; ++i) { + getline(file, value, ','); + imu(i) = atof(value.c_str()); + } + getline(file, value, '\n'); + imu(5) = atof(value.c_str()); + + // Adding the IMU preintegration. + preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); + + } else if (type == 1) { // GPS measurement + Vector7 gps; + for (int i = 0; i < 6; ++i) { + getline(file, value, ','); + gps(i) = atof(value.c_str()); + } + getline(file, value, '\n'); + gps(6) = atof(value.c_str()); + + index++; + + // Adding IMU factor and GPS factor and optimizing. + auto preint_imu_combined = + dynamic_cast( + *preintegrated); + CombinedImuFactor imu_factor(X(index - 1), V(index - 1), X(index), + V(index), B(index - 1), B(index), + preint_imu_combined); + graph.add(imu_factor); + + auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); + GPSFactor gps_factor(X(index), + Point3(gps(0), // N, + gps(1), // E, + gps(2)), // D, + correction_noise); + graph.add(gps_factor); + + // Now optimize and compare results. + prop_state = preintegrated->predict(prev_state, prev_bias); + initial_values.insert(X(index), prop_state.pose()); + initial_values.insert(V(index), prop_state.v()); + initial_values.insert(B(index), prev_bias); + + LevenbergMarquardtParams params; + params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtOptimizer optimizer(graph, initial_values, params); + Values result = optimizer.optimize(); + + // Overwrite the beginning of the preintegration for the next step. + prev_state = + NavState(result.at(X(index)), result.at(V(index))); + prev_bias = result.at(B(index)); + + // Reset the preintegration object. + preintegrated->resetIntegrationAndSetBias(prev_bias); + + // Print out the position and orientation error for comparison. + Vector3 result_position = prev_state.pose().translation(); + Vector3 position_error = result_position - gps.head<3>(); + current_position_error = position_error.norm(); + + Quaternion result_quat = prev_state.pose().rotation().toQuaternion(); + Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5)); + Quaternion quat_error = result_quat * gps_quat.inverse(); + quat_error.normalize(); + Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2, + quat_error.z() * 2); + current_orientation_error = euler_angle_error.norm(); + + // display statistics + cout << "Position error:" << current_position_error << "\t " + << "Angular error:" << current_orientation_error << "\n" << endl; + + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + output_time, result_position(0), result_position(1), + result_position(2), result_quat.x(), result_quat.y(), + result_quat.z(), result_quat.w(), gps(0), gps(1), gps(2), + gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); + + output_time += 1.0; + + } else { + cerr << "ERROR parsing file\n"; + return 1; + } + } + fclose(fp_out); + cout << "Complete, results written to " << output_filename << "\n\n"; + + return 0; +} From 3bc109de538c8281c25370ee61686816ad846fed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Aug 2020 00:26:34 -0500 Subject: [PATCH 12/48] renamed ImuFactorExample2 to be more consistent with naming --- examples/{ImuFactorExample2.cpp => ImuFactorsExample2.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{ImuFactorExample2.cpp => ImuFactorsExample2.cpp} (100%) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorsExample2.cpp similarity index 100% rename from examples/ImuFactorExample2.cpp rename to examples/ImuFactorsExample2.cpp From 311491abfc19a549397d907679c45d16a2a12753 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Aug 2020 00:29:29 -0500 Subject: [PATCH 13/48] revert author change --- cython/gtsam/examples/ImuFactorISAM2Example.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorISAM2Example.py b/cython/gtsam/examples/ImuFactorISAM2Example.py index 3d3138bf5..4187fe878 100644 --- a/cython/gtsam/examples/ImuFactorISAM2Example.py +++ b/cython/gtsam/examples/ImuFactorISAM2Example.py @@ -1,6 +1,6 @@ """ -iSAM2 example with ImuFactor. -Author: Frank Dellaert, Varun Agrawal +ImuFactor example with iSAM2. +Authors: Robert Truax (C++), Frank Dellaert, Varun Agrawal (Python) """ # pylint: disable=invalid-name, E1101 From 4949f8bb9d507de8cac51ebf75d48db8ed88d499 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Aug 2020 01:32:44 -0500 Subject: [PATCH 14/48] reworked basic ImuFactorsExample --- examples/ImuFactorsExample.cpp | 194 ++++++++++++++++----------------- 1 file changed, 92 insertions(+), 102 deletions(-) diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 315f7b8e1..fe1a99ed0 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -40,15 +40,15 @@ */ // GTSAM related includes. +#include #include #include #include -#include -#include +#include #include #include -#include -#include +#include +#include #include #include @@ -64,45 +64,75 @@ using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -static const char output_filename[] = "imuFactorExampleResults.csv"; -static const char use_combined_imu_flag[3] = "-c"; +boost::shared_ptr imuParams() { + // We use the sensor specs to build the noise model for the IMU factor. + double accel_noise_sigma = 0.0003924; + double gyro_noise_sigma = 0.000205689024915; + double accel_bias_rw_sigma = 0.004905; + double gyro_bias_rw_sigma = 0.000001454441043; + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration + + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + // PreintegrationBase params: + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + // should be using 2nd order integration + // PreintegratedRotation params: + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + // PreintegrationCombinedMeasurements params: + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccOmegaInt = bias_acc_omega_int; + + return p; +} int main(int argc, char* argv[]) { - string data_filename; - bool use_combined_imu = false; + string data_filename, output_filename; -#ifndef USE_LM - printf("Using ISAM2\n"); - ISAM2Params parameters; - parameters.relinearizeThreshold = 0.01; - parameters.relinearizeSkip = 1; - ISAM2 isam2(parameters); -#else - printf("Using Levenberg Marquardt Optimizer\n"); -#endif + bool use_isam = false; - if (argc < 2) { - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else if (argc < 3) { - if (strcmp(argv[1], use_combined_imu_flag) == 0) { - printf("using CombinedImuFactor\n"); - use_combined_imu = true; - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else { - data_filename = argv[1]; - } - } else { + if (argc == 4) { data_filename = argv[1]; - if (strcmp(argv[2], use_combined_imu_flag) == 0) { - printf("using CombinedImuFactor\n"); - use_combined_imu = true; - } + output_filename = argv[2]; + use_isam = atoi(argv[3]); + + } else if (argc == 3) { + data_filename = argv[1]; + output_filename = argv[2]; + } else if (argc == 2) { + data_filename = argv[1]; + output_filename = "imuFactorExampleResults.csv"; + } else { + printf("using default files\n"); + data_filename = findExampleDataFile("imuAndGPSdata.csv"); + output_filename = "imuFactorExampleResults.csv"; + } + + ISAM2* isam2; + if (use_isam) { + printf("Using ISAM2\n"); + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + isam2 = new ISAM2(parameters); + + } else { + printf("Using Levenberg Marquardt Optimizer\n"); } // Set up output file for plotting errors - FILE* fp_out = fopen(output_filename, "w+"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx," "gt_qy,gt_qz,gt_qw\n"); @@ -152,43 +182,11 @@ int main(int argc, char* argv[]) { graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model); graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model); - // We use the sensor specs to build the noise model for the IMU factor. - double accel_noise_sigma = 0.0003924; - double gyro_noise_sigma = 0.000205689024915; - double accel_bias_rw_sigma = 0.004905; - double gyro_bias_rw_sigma = 0.000001454441043; - Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); - Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); - Matrix33 integration_error_cov = - I_3x3 * 1e-8; // error committed in integrating position from velocities - Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); - Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = - I_6x6 * 1e-5; // error in the bias used for preintegration + auto p = imuParams(); - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); - // PreintegrationBase params: - p->accelerometerCovariance = - measured_acc_cov; // acc white noise in continuous - p->integrationCovariance = - integration_error_cov; // integration uncertainty continuous - // should be using 2nd order integration - // PreintegratedRotation params: - p->gyroscopeCovariance = - measured_omega_cov; // gyro white noise in continuous - // PreintegrationCombinedMeasurements params: - p->biasAccCovariance = bias_acc_cov; // acc bias in continuous - p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + std::shared_ptr preintegrated = + std::make_shared(p, prior_imu_bias); - std::shared_ptr preintegrated = nullptr; - if (use_combined_imu) { - preintegrated = - std::make_shared(p, prior_imu_bias); - } else { - preintegrated = - std::make_shared(p, prior_imu_bias); - } assert(preintegrated); // Store previous state for imu integration and latest predicted outcome. @@ -233,27 +231,16 @@ int main(int argc, char* argv[]) { correction_count++; // Adding IMU factor and GPS factor and optimizing. - if (use_combined_imu) { - auto preint_imu_combined = - dynamic_cast( - *preintegrated); - CombinedImuFactor imu_factor( - X(correction_count - 1), V(correction_count - 1), - X(correction_count), V(correction_count), B(correction_count - 1), - B(correction_count), preint_imu_combined); - graph->add(imu_factor); - } else { - auto preint_imu = - dynamic_cast(*preintegrated); - ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), - X(correction_count), V(correction_count), - B(correction_count - 1), preint_imu); - graph->add(imu_factor); - imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor( - B(correction_count - 1), B(correction_count), zero_bias, - bias_noise_model)); - } + auto preint_imu = + dynamic_cast(*preintegrated); + ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), + B(correction_count - 1), preint_imu); + graph->add(imu_factor); + imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + graph->add(BetweenFactor( + B(correction_count - 1), B(correction_count), zero_bias, + bias_noise_model)); auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); GPSFactor gps_factor(X(correction_count), @@ -270,18 +257,21 @@ int main(int argc, char* argv[]) { initial_values.insert(B(correction_count), prev_bias); Values result; -#ifdef USE_LM - LevenbergMarquardtOptimizer optimizer(*graph, initial_values); - result = optimizer.optimize(); -#else - isam2.update(*graph, initial_values); - isam2.update(); - result = isam2.calculateEstimate(); - // reset the graph - graph->resize(0); - initial_values.clear(); -#endif + if (use_isam) { + isam2->update(*graph, initial_values); + isam2->update(); + result = isam2->calculateEstimate(); + + // reset the graph + graph->resize(0); + initial_values.clear(); + + } else { + LevenbergMarquardtOptimizer optimizer(*graph, initial_values); + result = optimizer.optimize(); + } + // Overwrite the beginning of the preintegration for the next step. prev_state = NavState(result.at(X(correction_count)), result.at(V(correction_count))); From a1f6e1585af155692ae9611bbe4324f957b2e496 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Aug 2020 01:32:58 -0500 Subject: [PATCH 15/48] use boost program_options for command line parsing --- examples/CombinedImuFactorsExample.cpp | 48 +++++++++++++++-------- examples/ImuFactorsExample.cpp | 54 ++++++++++++++------------ 2 files changed, 62 insertions(+), 40 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index f04f2ddd4..2e5c46025 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -29,12 +29,11 @@ * Note that for GPS correction, we're only using the position not the * rotation. The rotation is provided in the file for ground truth comparison. * - * Usage: ./CombinedImuFactorsExample [data_csv_path] [-c] - * optional arguments: - * data_csv_path path to the CSV file with the IMU data. - * -c use CombinedImuFactor + * See usage: ./CombinedImuFactorsExample --help */ +#include + // GTSAM related includes. #include #include @@ -56,6 +55,29 @@ using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) +namespace po = boost::program_options; + +po::variables_map parseOptions(int argc, char* argv[]) { + po::options_description desc; + desc.add_options()("help,h", "produce help message")( + "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data")( + "output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use")("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (vm.count("help")) { + cout << desc << "\n"; + exit(1); + } + + return vm; +} + Vector10 readInitialState(ifstream& file) { string value; // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) @@ -106,17 +128,10 @@ boost::shared_ptr imuParams() { int main(int argc, char* argv[]) { string data_filename, output_filename; - if (argc == 3) { - data_filename = argv[1]; - output_filename = argv[2]; - } else if (argc == 2) { - data_filename = argv[1]; - output_filename = "imuFactorExampleResults.csv"; - } else { - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - output_filename = "imuFactorExampleResults.csv"; - } + po::variables_map var_map = parseOptions(argc, argv); + + data_filename = findExampleDataFile(var_map["data_csv_path"].as()); + output_filename = var_map["output_filename"].as(); // Set up output file for plotting errors FILE* fp_out = fopen(output_filename.c_str(), "w+"); @@ -265,7 +280,8 @@ int main(int argc, char* argv[]) { // display statistics cout << "Position error:" << current_position_error << "\t " - << "Angular error:" << current_orientation_error << "\n" << endl; + << "Angular error:" << current_orientation_error << "\n" + << endl; fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", output_time, result_position(0), result_position(1), diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index fe1a99ed0..20dc2c261 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -31,14 +31,11 @@ * Note that for GPS correction, we're only using the position not the * rotation. The rotation is provided in the file for ground truth comparison. * - * Usage: ./ImuFactorsExample [data_csv_path] [-c] - * optional arguments: - * data_csv_path path to the CSV file with the IMU data. - * -c use CombinedImuFactor - * Note: Define USE_LM to use Levenberg Marquardt Optimizer - * By default ISAM2 is used + * See usage: ./ImuFactorsExample --help */ +#include + // GTSAM related includes. #include #include @@ -54,9 +51,6 @@ #include #include -// Uncomment the following to use Levenberg Marquardt Optimizer -// #define USE_LM - using namespace gtsam; using namespace std; @@ -64,6 +58,29 @@ using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) +namespace po = boost::program_options; + +po::variables_map parseOptions(int argc, char* argv[]) { + po::options_description desc; + desc.add_options()("help,h", "produce help message")( + "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data")( + "output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use")("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (vm.count("help")) { + cout << desc << "\n"; + exit(1); + } + + return vm; +} + boost::shared_ptr imuParams() { // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; @@ -102,22 +119,11 @@ int main(int argc, char* argv[]) { bool use_isam = false; - if (argc == 4) { - data_filename = argv[1]; - output_filename = argv[2]; - use_isam = atoi(argv[3]); + po::variables_map var_map = parseOptions(argc, argv); - } else if (argc == 3) { - data_filename = argv[1]; - output_filename = argv[2]; - } else if (argc == 2) { - data_filename = argv[1]; - output_filename = "imuFactorExampleResults.csv"; - } else { - printf("using default files\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - output_filename = "imuFactorExampleResults.csv"; - } + data_filename = findExampleDataFile(var_map["data_csv_path"].as()); + output_filename = var_map["output_filename"].as(); + use_isam = var_map["use_isam"].as(); ISAM2* isam2; if (use_isam) { From 5a85494f33118d7aff796ae1446d13f65a8d7340 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 17 Aug 2020 13:12:40 -0400 Subject: [PATCH 16/48] replace atof/atoi with standardized stof/stoi --- examples/CombinedImuFactorsExample.cpp | 14 +++++++------- examples/ImuFactorsExample.cpp | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 2e5c46025..c9646e64d 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -85,10 +85,10 @@ Vector10 readInitialState(ifstream& file) { getline(file, value, ','); // i for (int i = 0; i < 9; i++) { getline(file, value, ','); - initial_state(i) = atof(value.c_str()); + initial_state(i) = stof(value.c_str()); } getline(file, value, '\n'); - initial_state(9) = atof(value.c_str()); + initial_state(9) = stof(value.c_str()); return initial_state; } @@ -204,16 +204,16 @@ int main(int argc, char* argv[]) { // Parse out first value string value; getline(file, value, ','); - int type = atoi(value.c_str()); + int type = stoi(value.c_str()); if (type == 0) { // IMU measurement Vector6 imu; for (int i = 0; i < 5; ++i) { getline(file, value, ','); - imu(i) = atof(value.c_str()); + imu(i) = stof(value.c_str()); } getline(file, value, '\n'); - imu(5) = atof(value.c_str()); + imu(5) = stof(value.c_str()); // Adding the IMU preintegration. preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); @@ -222,10 +222,10 @@ int main(int argc, char* argv[]) { Vector7 gps; for (int i = 0; i < 6; ++i) { getline(file, value, ','); - gps(i) = atof(value.c_str()); + gps(i) = stof(value.c_str()); } getline(file, value, '\n'); - gps(6) = atof(value.c_str()); + gps(6) = stof(value.c_str()); index++; diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 20dc2c261..a8a9715e0 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -154,10 +154,10 @@ int main(int argc, char* argv[]) { getline(file, value, ','); // i for (int i = 0; i < 9; i++) { getline(file, value, ','); - initial_state(i) = atof(value.c_str()); + initial_state(i) = stof(value.c_str()); } getline(file, value, '\n'); - initial_state(9) = atof(value.c_str()); + initial_state(9) = stof(value.c_str()); cout << "initial state:\n" << initial_state.transpose() << "\n\n"; // Assemble initial quaternion through GTSAM constructor @@ -211,16 +211,16 @@ int main(int argc, char* argv[]) { while (file.good()) { // Parse out first value getline(file, value, ','); - int type = atoi(value.c_str()); + int type = stoi(value.c_str()); if (type == 0) { // IMU measurement Vector6 imu; for (int i = 0; i < 5; ++i) { getline(file, value, ','); - imu(i) = atof(value.c_str()); + imu(i) = stof(value.c_str()); } getline(file, value, '\n'); - imu(5) = atof(value.c_str()); + imu(5) = stof(value.c_str()); // Adding the IMU preintegration. preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); @@ -229,10 +229,10 @@ int main(int argc, char* argv[]) { Vector7 gps; for (int i = 0; i < 6; ++i) { getline(file, value, ','); - gps(i) = atof(value.c_str()); + gps(i) = stof(value.c_str()); } getline(file, value, '\n'); - gps(6) = atof(value.c_str()); + gps(6) = stof(value.c_str()); correction_count++; From 3beb4df3a2b2caab1c61be9722284e4ff606fd09 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Sat, 22 Aug 2020 18:18:16 -0400 Subject: [PATCH 17/48] Replace MakeATangentVector with MakeATangentVectorValues --- gtsam/sfm/ShonanAveraging.cpp | 36 ++++++++++++------------- gtsam/sfm/ShonanAveraging.h | 7 ++--- gtsam/sfm/tests/testShonanAveraging.cpp | 6 ++--- 3 files changed, 25 insertions(+), 24 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 2485418cf..739fd9ebe 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -643,20 +643,26 @@ bool ShonanAveraging::checkOptimality(const Values &values) const { } /* ************************************************************************* */ -/// Create a tangent direction xi with eigenvector segment v_i +/// Create a VectorValues with eigenvector v template -Vector ShonanAveraging::MakeATangentVector(size_t p, const Vector &v, - size_t i) { +VectorValues ShonanAveraging::MakeATangentVectorValues(size_t p, + const Vector &v) { + VectorValues delta; // Create a tangent direction xi with eigenvector segment v_i const size_t dimension = SOn::Dimension(p); - const auto v_i = v.segment(d * i); - Vector xi = Vector::Zero(dimension); - double sign = pow(-1.0, round((p + 1) / 2) + 1); - for (size_t j = 0; j < d; j++) { - xi(j + p - d - 1) = sign * v_i(d - j - 1); - sign = -sign; + for (size_t i = 0; i < v.size() / d; i++) { + // Create a tangent direction xi with eigenvector segment v_i + // Assumes key is 0-based integer + const auto v_i = v.segment(d * i); + Vector xi = Vector::Zero(dimension); + double sign = pow(-1.0, round((p + 1) / 2) + 1); + for (size_t j = 0; j < d; j++) { + xi(j + p - d - 1) = sign * v_i(d - j - 1); + sign = -sign; + } + delta.insert(i, xi); } - return xi; + return delta; } /* ************************************************************************* */ @@ -690,14 +696,8 @@ template Values ShonanAveraging::LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector) { Values lifted = LiftTo(p, values); - for (auto it : lifted.filter()) { - // Create a tangent direction xi with eigenvector segment v_i - // Assumes key is 0-based integer - const Vector xi = MakeATangentVector(p, minEigenVector, it.key); - // Move the old value in the descent direction - it.value = it.value.retract(xi); - } - return lifted; + VectorValues delta = MakeATangentVectorValues(p, minEigenVector); + return lifted.retract(delta); } /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index ed94329a2..cada7cfd6 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -20,12 +20,13 @@ #include #include +#include #include #include +#include #include #include #include -#include #include #include @@ -200,8 +201,8 @@ public: /// Project pxdN Stiefel manifold matrix S to Rot3^N Values roundSolutionS(const Matrix &S) const; - /// Create a tangent direction xi with eigenvector segment v_i - static Vector MakeATangentVector(size_t p, const Vector &v, size_t i); + /// Create a VectorValues with eigenvector v_i + static VectorValues MakeATangentVectorValues(size_t p, const Vector &v); /// Calculate the riemannian gradient of F(values) at values Matrix riemannianGradient(size_t p, const Values &values) const; diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index cc4319e15..1cb5c06d1 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -121,7 +121,7 @@ TEST(ShonanAveraging3, tryOptimizingAt4) { } /* ************************************************************************* */ -TEST(ShonanAveraging3, MakeATangentVector) { +TEST(ShonanAveraging3, MakeATangentVectorValues) { Vector9 v; v << 1, 2, 3, 4, 5, 6, 7, 8, 9; Matrix expected(5, 5); @@ -130,8 +130,8 @@ TEST(ShonanAveraging3, MakeATangentVector) { 0, 0, 0, 0, -6, // 0, 0, 0, 0, 0, // 4, 5, 6, 0, 0; - const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1); - const auto actual = SOn::Hat(xi_1); + const VectorValues delta = ShonanAveraging3::MakeATangentVectorValues(5, v); + const auto actual = SOn::Hat(delta[1]); CHECK(assert_equal(expected, actual)); } From 6eb8b13f6ecbee000033bff649e8405f84f68b5c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 23 Aug 2020 20:06:21 -0400 Subject: [PATCH 18/48] Make sure our readme is proper restructuredText --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 99903f0b9..b9e85872c 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,9 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes Networks as the underlying computing paradigm rather than sparse matrices. + +The current support matrix is:: + | Platform | Compiler | Build Status | |:------------:|:---------:|:-------------:| | Ubuntu 18.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) | From bba3b6772aab87f40905249a581a893432fa8ff4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 23 Aug 2020 20:54:38 -0400 Subject: [PATCH 19/48] Remove unnecessary comment --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index b9e85872c..cbd7e35fe 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,6 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes Networks as the underlying computing paradigm rather than sparse matrices. - The current support matrix is:: | Platform | Compiler | Build Status | From b74c053c650f01a4b037753848af8342920c545e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 23 Aug 2020 22:01:49 -0400 Subject: [PATCH 20/48] Proper method to deal with markdown in setup.py --- README.md | 2 +- python/setup.py.in | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index cbd7e35fe..3982f5585 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes Networks as the underlying computing paradigm rather than sparse matrices. -The current support matrix is:: +The current support matrix is: | Platform | Compiler | Build Status | |:------------:|:---------:|:-------------:| diff --git a/python/setup.py.in b/python/setup.py.in index 55431a9ad..a7acdc4ba 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -27,6 +27,7 @@ setup( author_email='frank.dellaert@gtsam.org', license='Simplified BSD license', keywords='slam sam robotics localization mapping optimization', + long_description_content_type='text/markdown', long_description=readme_contents, # https://pypi.org/pypi?%3Aaction=list_classifiers classifiers=[ From 4dd9ee5f1fe39b888b51bc6671eefd59c7fdb061 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 24 Aug 2020 14:40:29 -0400 Subject: [PATCH 21/48] Linting and getAnchor wrap --- gtsam/gtsam.i | 2 + gtsam/sfm/ShonanAveraging.cpp | 115 +++++++++++++++++++--------------- gtsam/sfm/ShonanAveraging.h | 76 +++++++++++----------- 3 files changed, 108 insertions(+), 85 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b9ecf6f3b..9859c6d57 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2925,6 +2925,7 @@ class ShonanAveragingParameters2 { void setOptimalityThreshold(double value); double getOptimalityThreshold() const; void setAnchor(size_t index, const gtsam::Rot2& value); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); @@ -2940,6 +2941,7 @@ class ShonanAveragingParameters3 { void setOptimalityThreshold(double value); double getOptimalityThreshold() const; void setAnchor(size_t index, const gtsam::Rot3& value); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 2485418cf..bec48f30d 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -16,26 +16,25 @@ * @brief Shonan Averaging algorithm */ -#include - +#include #include #include #include #include #include +#include +#include #include #include #include -#include #include -#include - #include #include #include #include #include +#include #include namespace gtsam { @@ -50,8 +49,11 @@ template ShonanAveragingParameters::ShonanAveragingParameters( const LevenbergMarquardtParams &_lm, const std::string &method, double optimalityThreshold, double alpha, double beta, double gamma) - : lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha), - beta(beta), gamma(gamma) { + : lm(_lm), + optimalityThreshold(optimalityThreshold), + alpha(alpha), + beta(beta), + gamma(gamma) { // By default, we will do conjugate gradient lm.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -92,29 +94,40 @@ template struct ShonanAveragingParameters<3>; /* ************************************************************************* */ // Calculate number of unknown rotations referenced by measurements +// Throws exception of keys are not numbered as expected (may change in future). template -static size_t -NrUnknowns(const typename ShonanAveraging::Measurements &measurements) { +static size_t NrUnknowns( + const typename ShonanAveraging::Measurements &measurements) { + Key maxKey = 0; std::set keys; for (const auto &measurement : measurements) { - keys.insert(measurement.key1()); - keys.insert(measurement.key2()); + for (const Key &key : measurement.keys()) { + maxKey = std::max(key, maxKey); + keys.insert(key); + } } - return keys.size(); + size_t N = keys.size(); + if (maxKey != N - 1) { + throw std::invalid_argument( + "As of now, ShonanAveraging expects keys numbered 0..N-1"); + } + return N; } /* ************************************************************************* */ template ShonanAveraging::ShonanAveraging(const Measurements &measurements, const Parameters ¶meters) - : parameters_(parameters), measurements_(measurements), + : parameters_(parameters), + measurements_(measurements), nrUnknowns_(NrUnknowns(measurements)) { for (const auto &measurement : measurements_) { const auto &model = measurement.noiseModel(); if (model && model->dim() != SO::dimension) { measurement.print("Factor with incorrect noise model:\n"); - throw std::invalid_argument("ShonanAveraging: measurements passed to " - "constructor have incorrect dimension."); + throw std::invalid_argument( + "ShonanAveraging: measurements passed to " + "constructor have incorrect dimension."); } } Q_ = buildQ(); @@ -196,7 +209,7 @@ Matrix ShonanAveraging::StiefelElementMatrix(const Values &values) { Matrix S(p, N * d); for (const auto it : values.filter()) { S.middleCols(it.key * d) = - it.value.matrix().leftCols(); // project Qj to Stiefel + it.value.matrix().leftCols(); // project Qj to Stiefel } return S; } @@ -227,7 +240,8 @@ Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const { } /* ************************************************************************* */ -template static Matrix RoundSolutionS(const Matrix &S) { +template +static Matrix RoundSolutionS(const Matrix &S) { const size_t N = S.cols() / d; // First, compute a thin SVD of S Eigen::JacobiSVD svd(S, Eigen::ComputeThinV); @@ -246,8 +260,7 @@ template static Matrix RoundSolutionS(const Matrix &S) { for (size_t i = 0; i < N; ++i) { // Compute the determinant of the ith dxd block of R double determinant = R.middleCols(d * i).determinant(); - if (determinant > 0) - ++numPositiveBlocks; + if (determinant > 0) ++numPositiveBlocks; } if (numPositiveBlocks < N / 2) { @@ -263,7 +276,8 @@ template static Matrix RoundSolutionS(const Matrix &S) { } /* ************************************************************************* */ -template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { +template <> +Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { // Round to a 2*2N matrix Matrix R = RoundSolutionS<2>(S); @@ -276,7 +290,8 @@ template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { return values; } -template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const { +template <> +Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const { // Round to a 3*3N matrix Matrix R = RoundSolutionS<3>(S); @@ -332,7 +347,8 @@ static double Kappa(const BinaryMeasurement &measurement) { } /* ************************************************************************* */ -template Sparse ShonanAveraging::buildD() const { +template +Sparse ShonanAveraging::buildD() const { // Each measurement contributes 2*d elements along the diagonal of the // degree matrix. static constexpr size_t stride = 2 * d; @@ -366,7 +382,8 @@ template Sparse ShonanAveraging::buildD() const { } /* ************************************************************************* */ -template Sparse ShonanAveraging::buildQ() const { +template +Sparse ShonanAveraging::buildQ() const { // Each measurement contributes 2*d^2 elements on a pair of symmetric // off-diagonal blocks static constexpr size_t stride = 2 * d * d; @@ -513,12 +530,12 @@ struct MatrixProdFunctor { // - We've been using 10^-4 for the nonnegativity tolerance // - for numLanczosVectors, 20 is a good default value -static bool -SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, - Vector *minEigenVector = 0, size_t *numIterations = 0, - size_t maxIterations = 1000, - double minEigenvalueNonnegativityTolerance = 10e-4, - Eigen::Index numLanczosVectors = 20) { +static bool SparseMinimumEigenValue( + const Sparse &A, const Matrix &S, double *minEigenValue, + Vector *minEigenVector = 0, size_t *numIterations = 0, + size_t maxIterations = 1000, + double minEigenvalueNonnegativityTolerance = 10e-4, + Eigen::Index numLanczosVectors = 20) { // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos MatrixProdFunctor lmOperator(A); Spectra::SymEigsSolvernormalize(); // Ensure that this is a unit vector + minEigenVector->normalize(); // Ensure that this is a unit vector } return true; } @@ -578,7 +594,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, Vector perturbation(v0.size()); perturbation.setRandom(); perturbation.normalize(); - Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3% + Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3% // Use this to initialize the eigensolver minEigenValueSolver.init(xinit.data()); @@ -590,21 +606,20 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN); - if (minConverged != 1) - return false; + if (minConverged != 1) return false; *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue; if (minEigenVector) { *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0); - minEigenVector->normalize(); // Ensure that this is a unit vector + minEigenVector->normalize(); // Ensure that this is a unit vector } - if (numIterations) - *numIterations = minEigenValueSolver.num_iterations(); + if (numIterations) *numIterations = minEigenValueSolver.num_iterations(); return true; } /* ************************************************************************* */ -template Sparse ShonanAveraging::computeA(const Matrix &S) const { +template +Sparse ShonanAveraging::computeA(const Matrix &S) const { auto Lambda = computeLambda(S); return Lambda - Q_; } @@ -628,8 +643,8 @@ double ShonanAveraging::computeMinEigenValue(const Values &values, /* ************************************************************************* */ template -std::pair -ShonanAveraging::computeMinEigenVector(const Values &values) const { +std::pair ShonanAveraging::computeMinEigenVector( + const Values &values) const { Vector minEigenVector; double minEigenValue = computeMinEigenValue(values, &minEigenVector); return std::make_pair(minEigenValue, minEigenVector); @@ -750,7 +765,8 @@ Values ShonanAveraging::initializeRandomly(std::mt19937 &rng) const { } /* ************************************************************************* */ -template Values ShonanAveraging::initializeRandomly() const { +template +Values ShonanAveraging::initializeRandomly() const { return initializeRandomly(kRandomNumberGenerator); } @@ -759,7 +775,7 @@ template Values ShonanAveraging::initializeRandomlyAt(size_t p, std::mt19937 &rng) const { const Values randomRotations = initializeRandomly(rng); - return LiftTo(p, randomRotations); // lift to p! + return LiftTo(p, randomRotations); // lift to p! } /* ************************************************************************* */ @@ -823,8 +839,8 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) // Extract Rot3 measurement from Pose3 betweenfactors // Modeled after similar function in dataset.cpp -static BinaryMeasurement -convert(const BetweenFactor::shared_ptr &f) { +static BinaryMeasurement convert( + const BetweenFactor::shared_ptr &f) { auto gaussian = boost::dynamic_pointer_cast(f->noiseModel()); if (!gaussian) @@ -837,12 +853,11 @@ convert(const BetweenFactor::shared_ptr &f) { noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); } -static ShonanAveraging3::Measurements -extractRot3Measurements(const BetweenFactorPose3s &factors) { +static ShonanAveraging3::Measurements extractRot3Measurements( + const BetweenFactorPose3s &factors) { ShonanAveraging3::Measurements result; result.reserve(factors.size()); - for (auto f : factors) - result.push_back(convert(f)); + for (auto f : factors) result.push_back(convert(f)); return result; } @@ -851,4 +866,4 @@ ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {} /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index ed94329a2..97f92d102 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -20,36 +20,38 @@ #include #include +#include #include #include #include #include #include -#include #include #include #include #include #include +#include namespace gtsam { class NonlinearFactorGraph; class LevenbergMarquardtOptimizer; /// Parameters governing optimization etc. -template struct GTSAM_EXPORT ShonanAveragingParameters { +template +struct GTSAM_EXPORT ShonanAveragingParameters { // Select Rot2 or Rot3 interface based template parameter d using Rot = typename std::conditional::type; using Anchor = std::pair; // Paremeters themselves: - LevenbergMarquardtParams lm; // LM parameters - double optimalityThreshold; // threshold used in checkOptimality - Anchor anchor; // pose to use as anchor if not Karcher - double alpha; // weight of anchor-based prior (default 0) - double beta; // weight of Karcher-based prior (default 1) - double gamma; // weight of gauge-fixing factors (default 0) + LevenbergMarquardtParams lm; // LM parameters + double optimalityThreshold; // threshold used in checkOptimality + Anchor anchor; // pose to use as anchor if not Karcher + double alpha; // weight of anchor-based prior (default 0) + double beta; // weight of Karcher-based prior (default 1) + double gamma; // weight of gauge-fixing factors (default 0) ShonanAveragingParameters(const LevenbergMarquardtParams &lm = LevenbergMarquardtParams::CeresDefaults(), @@ -64,6 +66,7 @@ template struct GTSAM_EXPORT ShonanAveragingParameters { double getOptimalityThreshold() const { return optimalityThreshold; } void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } + std::pair getAnchor() { return anchor; } void setAnchorWeight(double value) { alpha = value; } double getAnchorWeight() { return alpha; } @@ -93,8 +96,9 @@ using ShonanAveragingParameters3 = ShonanAveragingParameters<3>; * European Computer Vision Conference, 2020. * You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0 */ -template class GTSAM_EXPORT ShonanAveraging { -public: +template +class GTSAM_EXPORT ShonanAveraging { + public: using Sparse = Eigen::SparseMatrix; // Define the Parameters type and use its typedef of the rotation type: @@ -105,13 +109,13 @@ public: // TODO(frank): use BinaryMeasurement? using Measurements = std::vector>; -private: + private: Parameters parameters_; Measurements measurements_; size_t nrUnknowns_; - Sparse D_; // Sparse (diagonal) degree matrix - Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr - Sparse L_; // connection Laplacian L = D - Q, needed for optimality check + Sparse D_; // Sparse (diagonal) degree matrix + Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr + Sparse L_; // connection Laplacian L = D - Q, needed for optimality check /** * Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as @@ -122,7 +126,7 @@ private: /// Build 3Nx3N sparse degree matrix D Sparse buildD() const; -public: + public: /// @name Standard Constructors /// @{ @@ -156,12 +160,12 @@ public: /// @name Matrix API (advanced use, debugging) /// @{ - Sparse D() const { return D_; } ///< Sparse version of D - Matrix denseD() const { return Matrix(D_); } ///< Dense version of D - Sparse Q() const { return Q_; } ///< Sparse version of Q - Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q - Sparse L() const { return L_; } ///< Sparse version of L - Matrix denseL() const { return Matrix(L_); } ///< Dense version of L + Sparse D() const { return D_; } ///< Sparse version of D + Matrix denseD() const { return Matrix(D_); } ///< Dense version of D + Sparse Q() const { return Q_; } ///< Sparse version of Q + Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q + Sparse L() const { return L_; } ///< Sparse version of L + Matrix denseL() const { return Matrix(L_); } ///< Dense version of L /// Version that takes pxdN Stiefel manifold elements Sparse computeLambda(const Matrix &S) const; @@ -220,11 +224,10 @@ public: * @param minEigenVector corresponding to minEigenValue at level p-1 * @return values of type SO(p) */ - Values - initializeWithDescent(size_t p, const Values &values, - const Vector &minEigenVector, double minEigenValue, - double gradienTolerance = 1e-2, - double preconditionedGradNormTolerance = 1e-4) const; + Values initializeWithDescent( + size_t p, const Values &values, const Vector &minEigenVector, + double minEigenValue, double gradienTolerance = 1e-2, + double preconditionedGradNormTolerance = 1e-4) const; /// @} /// @name Advanced API /// @{ @@ -237,11 +240,11 @@ public: /** * Create initial Values of type SO(p) - * @param p the dimensionality of the rotation manifold + * @param p the dimensionality of the rotation manifold */ Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const; - /// Version of initializeRandomlyAt with fixed random seed. + /// Version of initializeRandomlyAt with fixed random seed. Values initializeRandomlyAt(size_t p) const; /** @@ -300,7 +303,8 @@ public: Values roundSolution(const Values &values) const; /// Lift Values of type T to SO(p) - template static Values LiftTo(size_t p, const Values &values) { + template + static Values LiftTo(size_t p, const Values &values) { Values result; for (const auto it : values.filter()) { result.insert(it.key, SOn::Lift(p, it.value.matrix())); @@ -327,7 +331,7 @@ public: */ Values initializeRandomly(std::mt19937 &rng) const; - /// Random initialization for wrapper, fixed random seed. + /// Random initialization for wrapper, fixed random seed. Values initializeRandomly() const; /** @@ -346,20 +350,22 @@ public: // convenience interface with file access. class ShonanAveraging2 : public ShonanAveraging<2> { -public: + public: ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters = Parameters()); - ShonanAveraging2(string g2oFile, const Parameters ¶meters = Parameters()); + explicit ShonanAveraging2(string g2oFile, + const Parameters ¶meters = Parameters()); }; class ShonanAveraging3 : public ShonanAveraging<3> { -public: + public: ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters = Parameters()); - ShonanAveraging3(string g2oFile, const Parameters ¶meters = Parameters()); + explicit ShonanAveraging3(string g2oFile, + const Parameters ¶meters = Parameters()); // TODO(frank): Deprecate after we land pybind wrapper ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters = Parameters()); }; -} // namespace gtsam +} // namespace gtsam From df73414b1ce17b40fd576ab7887be66e73da8e6e Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 24 Aug 2020 23:18:43 -0400 Subject: [PATCH 22/48] Fixed checkwitheigen testcase failure --- gtsam/sfm/tests/testShonanAveraging.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 1cb5c06d1..f359c3f81 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -158,17 +158,17 @@ TEST(ShonanAveraging3, CheckWithEigen) { double lambda = kShonan.computeMinEigenValue(Qstar3); // Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix! - const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3); - auto A = kShonan.computeA(S); - bool computeEigenvectors = false; - Eigen::EigenSolver eigenSolver(Matrix(A), computeEigenvectors); - auto lambdas = eigenSolver.eigenvalues().real(); - double minEigenValue = lambdas(0); - for (int i = 1; i < lambdas.size(); i++) - minEigenValue = min(lambdas(i), minEigenValue); + // const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3); + // auto A = kShonan.computeA(S); + // bool computeEigenvectors = false; + // Eigen::EigenSolver eigenSolver(Matrix(A), computeEigenvectors); + // auto lambdas = eigenSolver.eigenvalues().real(); + // double minEigenValue = lambdas(0); + // for (int i = 1; i < lambdas.size(); i++) + // minEigenValue = min(lambdas(i), minEigenValue); // Actual check - EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12); + EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11); // Construct test descent direction (as minEigenVector is not predictable // across platforms, being one from a basically flat 3d- subspace) From 8c6082eb64a5456a57a422beebebf7e1b3e58646 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Tue, 25 Aug 2020 10:23:26 -0400 Subject: [PATCH 23/48] added missing namespace --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 9859c6d57..79d489128 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2925,7 +2925,7 @@ class ShonanAveragingParameters2 { void setOptimalityThreshold(double value); double getOptimalityThreshold() const; void setAnchor(size_t index, const gtsam::Rot2& value); - pair getAnchor(); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); @@ -2941,7 +2941,7 @@ class ShonanAveragingParameters3 { void setOptimalityThreshold(double value); double getOptimalityThreshold() const; void setAnchor(size_t index, const gtsam::Rot3& value); - pair getAnchor(); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); From 491405a5f13ef91ab0756fa98d7209425e0ebe82 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 25 Aug 2020 14:47:08 -0400 Subject: [PATCH 24/48] Rename and small fix. --- gtsam/sfm/ShonanAveraging.cpp | 11 +++++------ gtsam/sfm/ShonanAveraging.h | 2 +- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 739fd9ebe..05f7f4bcc 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -643,19 +643,18 @@ bool ShonanAveraging::checkOptimality(const Values &values) const { } /* ************************************************************************* */ -/// Create a VectorValues with eigenvector v template -VectorValues ShonanAveraging::MakeATangentVectorValues(size_t p, - const Vector &v) { +VectorValues ShonanAveraging::TangentVectorValues(size_t p, + const Vector &v) { VectorValues delta; // Create a tangent direction xi with eigenvector segment v_i const size_t dimension = SOn::Dimension(p); + double sign0 = pow(-1.0, round((p + 1) / 2) + 1); for (size_t i = 0; i < v.size() / d; i++) { - // Create a tangent direction xi with eigenvector segment v_i // Assumes key is 0-based integer const auto v_i = v.segment(d * i); Vector xi = Vector::Zero(dimension); - double sign = pow(-1.0, round((p + 1) / 2) + 1); + double sign = sign0; for (size_t j = 0; j < d; j++) { xi(j + p - d - 1) = sign * v_i(d - j - 1); sign = -sign; @@ -696,7 +695,7 @@ template Values ShonanAveraging::LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector) { Values lifted = LiftTo(p, values); - VectorValues delta = MakeATangentVectorValues(p, minEigenVector); + VectorValues delta = TangentVectorValues(p, minEigenVector); return lifted.retract(delta); } diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index cada7cfd6..3fee780dd 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -202,7 +202,7 @@ public: Values roundSolutionS(const Matrix &S) const; /// Create a VectorValues with eigenvector v_i - static VectorValues MakeATangentVectorValues(size_t p, const Vector &v); + static VectorValues TangentVectorValues(size_t p, const Vector &v); /// Calculate the riemannian gradient of F(values) at values Matrix riemannianGradient(size_t p, const Values &values) const; From 1980dcf1f5f187ab75788903124f43eb6e229c70 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 25 Aug 2020 14:48:25 -0400 Subject: [PATCH 25/48] Changed TangentVectorValues test --- gtsam/sfm/tests/testShonanAveraging.cpp | 36 ++++++++++++------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index f359c3f81..7cd2af860 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -121,18 +121,17 @@ TEST(ShonanAveraging3, tryOptimizingAt4) { } /* ************************************************************************* */ -TEST(ShonanAveraging3, MakeATangentVectorValues) { +TEST(ShonanAveraging3, TangentVectorValues) { Vector9 v; v << 1, 2, 3, 4, 5, 6, 7, 8, 9; - Matrix expected(5, 5); - expected << 0, 0, 0, 0, -4, // - 0, 0, 0, 0, -5, // - 0, 0, 0, 0, -6, // - 0, 0, 0, 0, 0, // - 4, 5, 6, 0, 0; - const VectorValues delta = ShonanAveraging3::MakeATangentVectorValues(5, v); - const auto actual = SOn::Hat(delta[1]); - CHECK(assert_equal(expected, actual)); + Vector expected0(10), expected1(10), expected2(10); + expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0; + expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0; + expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0; + const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v); + EXPECT(assert_equal(expected0, xi[0])); + EXPECT(assert_equal(expected1, xi[1])); + EXPECT(assert_equal(expected2, xi[2])); } /* ************************************************************************* */ @@ -158,17 +157,18 @@ TEST(ShonanAveraging3, CheckWithEigen) { double lambda = kShonan.computeMinEigenValue(Qstar3); // Check Eigenvalue with slow Eigen version, converts matrix A to dense matrix! - // const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3); - // auto A = kShonan.computeA(S); - // bool computeEigenvectors = false; - // Eigen::EigenSolver eigenSolver(Matrix(A), computeEigenvectors); - // auto lambdas = eigenSolver.eigenvalues().real(); - // double minEigenValue = lambdas(0); - // for (int i = 1; i < lambdas.size(); i++) - // minEigenValue = min(lambdas(i), minEigenValue); + const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3); + auto A = kShonan.computeA(S); + bool computeEigenvectors = false; + Eigen::EigenSolver eigenSolver(Matrix(A), computeEigenvectors); + auto lambdas = eigenSolver.eigenvalues().real(); + double minEigenValue = lambdas(0); + for (int i = 1; i < lambdas.size(); i++) + minEigenValue = min(lambdas(i), minEigenValue); // Actual check EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11); + EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11); // Construct test descent direction (as minEigenVector is not predictable // across platforms, being one from a basically flat 3d- subspace) From 62098fb5840d94b483b9708413029e1cbd0eb336 Mon Sep 17 00:00:00 2001 From: Christian Berg Date: Sun, 30 Aug 2020 17:38:07 +0200 Subject: [PATCH 26/48] Added Jacobians for Rot3::RzRyRx and related named constructors --- gtsam/geometry/Rot3.h | 24 ++++++++++++--- gtsam/geometry/Rot3M.cpp | 6 +++- gtsam/geometry/tests/testRot3.cpp | 51 +++++++++++++++++++++++++++++++ 3 files changed, 76 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 33d120a85..ae69fc57f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -154,12 +154,23 @@ namespace gtsam { static Rot3 Rz(double t); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. - static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(double x, double y, double z, + OptionalJacobian<3, 1> Hx = boost::none, + OptionalJacobian<3, 1> Hy = boost::none, + OptionalJacobian<3, 1> Hz = boost::none); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. - inline static Rot3 RzRyRx(const Vector& xyz) { + inline static Rot3 RzRyRx(const Vector& xyz, + OptionalJacobian<3, 3> H = boost::none) { assert(xyz.size() == 3); - return RzRyRx(xyz(0), xyz(1), xyz(2)); + Rot3 out; + if (H) { + Vector3 Hx, Hy, Hz; + out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz); + (*H) << Hx, Hy, Hz; + } else + out = RzRyRx(xyz(0), xyz(1), xyz(2)); + return out; } /// Positive yaw is to right (as in aircraft heading). See ypr @@ -185,7 +196,12 @@ namespace gtsam { * Positive pitch is down (decreasing aircraft altitude). * Positive roll is to right (decreasing yaw in aircraft). */ - static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 Ypr(double y, double p, double r, + OptionalJacobian<3, 1> Hy = boost::none, + OptionalJacobian<3, 1> Hp = boost::none, + OptionalJacobian<3, 1> Hr = boost::none) { + return RzRyRx(r, p, y, Hr, Hp, Hy); + } /** Create from Quaternion coefficients */ static Rot3 Quaternion(double w, double x, double y, double z) { diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 46a07e50a..500941a16 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -82,7 +82,8 @@ Rot3 Rot3::Rz(double t) { /* ************************************************************************* */ // Considerably faster than composing matrices above ! -Rot3 Rot3::RzRyRx(double x, double y, double z) { +Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, + OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) { double cx=cos(x),sx=sin(x); double cy=cos(y),sy=sin(y); double cz=cos(z),sz=sin(z); @@ -97,6 +98,9 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { double s_c = sx * cz; double c_c = cx * cz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + if (Hx) (*Hx) << 1, 0, 0; + if (Hy) (*Hy) << 0, cx, -sx; + if (Hz) (*Hz) << -sy, sc_, cc_; return Rot3( _cc,- c_s + ssc, s_s + csc, _cs, c_c + sss, -s_c + css, diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d5400494e..fba84c724 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -743,6 +743,57 @@ TEST(Rot3, axisAngle) { CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195)) } +/* ************************************************************************* */ +Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) { + return Rot3::RzRyRx(a, b, c); +} + +TEST(Rot3, RzRyRx_scalars_derivative) { + const auto x = 0.1, y = 0.4, z = 0.2; + const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z); + const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z); + const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z); + + Vector3 act_x, act_y, act_z; + Rot3::RzRyRx(x, y, z, act_x, act_y, act_z); + + CHECK(assert_equal(num_x, act_x)); + CHECK(assert_equal(num_y, act_y)); + CHECK(assert_equal(num_z, act_z)); +} + +/* ************************************************************************* */ +Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); } + +TEST(Rot3, RzRyRx_vector_derivative) { + const auto xyz = Vector3{-0.3, 0.1, 0.7}; + const auto num = numericalDerivative11(RzRyRx_proxy, xyz); + + Matrix3 act; + Rot3::RzRyRx(xyz, act); + + CHECK(assert_equal(num, act)); +} + +/* ************************************************************************* */ +Rot3 Ypr_proxy(double const& y, double const& p, double const& r) { + return Rot3::Ypr(y, p, r); +} + +TEST(Rot3, Ypr_derivative) { + const auto y = 0.7, p = -0.3, r = 0.1; + const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r); + const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r); + const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r); + + Vector3 act_y, act_p, act_r; + Rot3::Ypr(y, p, r, act_y, act_p, act_r); + + CHECK(assert_equal(num_y, act_y)); + CHECK(assert_equal(num_p, act_p)); + CHECK(assert_equal(num_r, act_r)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 13d33e0f51c08932f0ed719ed75aa486238ab882 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 09:50:10 -0400 Subject: [PATCH 27/48] Define dataset directory for Python package --- CMakeLists.txt | 8 ++++++++ gtsam/config.h.in | 1 + 2 files changed, 9 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dce69903..8a8b6ce01 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -428,6 +428,14 @@ add_subdirectory(CppUnitLite) if(GTSAM_BUILD_PYTHON) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") add_subdirectory(python) + if(CMAKE_SYSTEM_NAME STREQUAL "Linux" OR CMAKE_SYSTEM_NAME STREQUAL "Darwin") + # Common directory which always has write access + # This is because the exact location of the Python installation may vary + # depending on various factors such as python version and use of virtual envs. + set(GTSAM_PYTHON_DATASET_DIR "/tmp/gtsam_example_data") + elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows") + set(GTSAM_PYTHON_DATASET_DIR "$ENV{LOCALAPPDATA}/gtsam_example_data") + endif() endif() # Build GTSAM library diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 9d1bd4ebd..7f5be65b0 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -27,6 +27,7 @@ // Paths to example datasets distributed with GTSAM #define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" +#define GTSAM_PYTHON_DATASET_DIR "@GTSAM_PYTHON_DATASET_DIR" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) #cmakedefine GTSAM_USE_QUATERNIONS From a7f8ba7ac4a0430e06492699bba97b965a151213 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 09:50:47 -0400 Subject: [PATCH 28/48] Add python dataset directory to search options --- gtsam/slam/dataset.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4bd7bc7e2..de8150324 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -69,6 +69,7 @@ string findExampleDataFile(const string &name) { // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); + rootsToSearch.push_back(GTSAM_PYTHON_DATASET_DIR); // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; From 3674b9c65f77638eaf46ec7241b5c8da6b1ada78 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 09:51:14 -0400 Subject: [PATCH 29/48] Add data files to python package --- python/setup.py.in | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/python/setup.py.in b/python/setup.py.in index a7acdc4ba..1ffe978f3 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -1,3 +1,4 @@ +import glob import os import sys @@ -8,6 +9,11 @@ except ImportError: packages = find_packages(where=".") print("PACKAGES: ", packages) + +data_path = '${GTSAM_SOURCE_DIR}/examples/Data/' +data_files_and_directories = glob.glob(data_path + '**', recursive=True) +data_files = [x for x in data_files_and_directories if not os.path.isdir(x)] + package_data = { '': [ './*.so', @@ -44,6 +50,7 @@ setup( ], packages=packages, package_data=package_data, + data_files=[('${GTSAM_PYTHON_DATASET_DIR}', data_files),], test_suite="gtsam.tests", install_requires=["numpy"], zip_safe=False, From ac3e564293c23aca557ecb7537dd4928b99721c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 10:10:54 -0400 Subject: [PATCH 30/48] Better location for setting Python dataset directory in CMake --- CMakeLists.txt | 8 -------- python/CMakeLists.txt | 11 +++++++++++ 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a8b6ce01..9dce69903 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -428,14 +428,6 @@ add_subdirectory(CppUnitLite) if(GTSAM_BUILD_PYTHON) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") add_subdirectory(python) - if(CMAKE_SYSTEM_NAME STREQUAL "Linux" OR CMAKE_SYSTEM_NAME STREQUAL "Darwin") - # Common directory which always has write access - # This is because the exact location of the Python installation may vary - # depending on various factors such as python version and use of virtual envs. - set(GTSAM_PYTHON_DATASET_DIR "/tmp/gtsam_example_data") - elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows") - set(GTSAM_PYTHON_DATASET_DIR "$ENV{LOCALAPPDATA}/gtsam_example_data") - endif() endif() # Build GTSAM library diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 23898f61d..6fad7ba45 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -4,6 +4,17 @@ if (NOT GTSAM_BUILD_PYTHON) return() endif() +# Common directory for storing data stored with the package. +# This is because the exact location of the Python installation may vary +# depending on various factors such as python version and use of virtual envs. +if(CMAKE_SYSTEM_NAME STREQUAL "Linux" OR CMAKE_SYSTEM_NAME STREQUAL "Darwin") + # Easy directory with write access. + set(GTSAM_PYTHON_DATASET_DIR "/tmp/gtsam_example_data") +elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows") + # %LocalAppData% directory in Windows. + set(GTSAM_PYTHON_DATASET_DIR "$ENV{LOCALAPPDATA}/gtsam_example_data") +endif() + # Generate setup.py. file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in From 63327a7c9d6b86880e3ed63ded8295fa12bb7a4c Mon Sep 17 00:00:00 2001 From: Christian Berg Date: Mon, 31 Aug 2020 18:49:18 +0200 Subject: [PATCH 31/48] Also add jacobians to Rot3::RzRyRx for when quaternions are selected --- gtsam/geometry/Rot3Q.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d609c289c..6e1871c64 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -67,10 +67,23 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, + OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) { + if (Hx) (*Hx) << 1, 0, 0; + + if (Hy or Hz) { + const auto cx = cos(x), sx = sin(x); + if (Hy) (*Hy) << 0, cx, -sx; + if (Hz) { + const auto cy = cos(y), sy = sin(y); + (*Hz) << -sy, sx * cy, cx * cy; + } + } + + return Rot3( + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ From 1cba2dc3e7996e3422b357373971eaea1d3bb9cf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 13:06:32 -0400 Subject: [PATCH 32/48] Print Vectors horizontally for easier reading --- gtsam/geometry/Point2.cpp | 6 ++++++ gtsam/geometry/Point2.h | 2 ++ gtsam/geometry/Point3.cpp | 6 ++++++ gtsam/geometry/Point3.h | 2 ++ gtsam/navigation/tests/testNavState.cpp | 2 +- 5 files changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index d8060cfcf..8f2b6c3e2 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -23,6 +23,12 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << p.transpose(); + return os; +} + /* ************************************************************************* */ double norm2(const Point2& p, OptionalJacobian<1,2> H) { double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index e6574fe41..312e65b3b 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -26,6 +26,8 @@ namespace gtsam { /// it is now possible to just typedef Point2 to Vector2 typedef Vector2 Point2; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2 &p); + /// Distance of the point from the origin, with Jacobian GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 7a46f5988..c976f8bcc 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -22,6 +22,12 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point3& p) { + os << p.transpose(); + return os; +} + /* ************************************************************************* */ double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 19e328022..df0feff4f 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,6 +33,8 @@ namespace gtsam { /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3 &p); + // Convenience typedef typedef std::pair Point3Pair; GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index d38b76255..48dd3bc3e 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -215,7 +215,7 @@ TEST(NavState, Stream) os << state; string expected; - expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0"; + expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0 0 0\nv: 0 0 0"; EXPECT(os.str() == expected); } From b23cdca66a4693008687251eba2e3a846ca10c9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 13:55:09 -0400 Subject: [PATCH 33/48] Undo previous changes for common directory --- gtsam/config.h.in | 1 - gtsam/slam/dataset.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 7f5be65b0..9d1bd4ebd 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -27,7 +27,6 @@ // Paths to example datasets distributed with GTSAM #define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" -#define GTSAM_PYTHON_DATASET_DIR "@GTSAM_PYTHON_DATASET_DIR" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) #cmakedefine GTSAM_USE_QUATERNIONS diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index de8150324..4bd7bc7e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -69,7 +69,6 @@ string findExampleDataFile(const string &name) { // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); - rootsToSearch.push_back(GTSAM_PYTHON_DATASET_DIR); // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; From 47800f31127c23a22dc55734b7bc0c221007141e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 13:55:21 -0400 Subject: [PATCH 34/48] don't wrap findExampleDataFile --- gtsam/gtsam.i | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 79d489128..33d8c11fe 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2800,7 +2800,6 @@ class SfmData { gtsam::SfmTrack track(size_t idx) const; }; -string findExampleDataFile(string name); pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); pair load2D(string filename, From 43dfe0f47d8e001572b7cb2471f5ca90515e2dfd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 13:56:56 -0400 Subject: [PATCH 35/48] Add python version of findExampleDataFile --- python/CMakeLists.txt | 13 +++---------- python/gtsam/__init__.py | 2 ++ python/gtsam/utils/__init__.py | 22 ++++++++++++++++++++++ 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 6fad7ba45..e15e571f5 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -4,16 +4,9 @@ if (NOT GTSAM_BUILD_PYTHON) return() endif() -# Common directory for storing data stored with the package. -# This is because the exact location of the Python installation may vary -# depending on various factors such as python version and use of virtual envs. -if(CMAKE_SYSTEM_NAME STREQUAL "Linux" OR CMAKE_SYSTEM_NAME STREQUAL "Darwin") - # Easy directory with write access. - set(GTSAM_PYTHON_DATASET_DIR "/tmp/gtsam_example_data") -elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows") - # %LocalAppData% directory in Windows. - set(GTSAM_PYTHON_DATASET_DIR "$ENV{LOCALAPPDATA}/gtsam_example_data") -endif() +# Common directory for storing data/datasets stored with the package. +# This will store the data in the Python site package directly. +set(GTSAM_PYTHON_DATASET_DIR "./gtsam/Data") # Generate setup.py. file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index b2d1fb7e7..70a00c3dc 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,4 +1,6 @@ +from . import utils from .gtsam import * +from .utils import findExampleDataFile def _init(): diff --git a/python/gtsam/utils/__init__.py b/python/gtsam/utils/__init__.py index e69de29bb..5ee733ba4 100644 --- a/python/gtsam/utils/__init__.py +++ b/python/gtsam/utils/__init__.py @@ -0,0 +1,22 @@ +import glob +import os.path as osp + + +def findExampleDataFile(name): + """ + Find the example data file specified by `name`. + """ + # This is okay since gtsam will already be loaded + # before this function is accessed. Thus no effect. + import gtsam + + site_package_path = gtsam.__path__[0] + # add the * at the end to glob the entire directory + data_path = osp.join(site_package_path, "Data", "*") + + extensions = ("", ".graph", ".txt", ".out", ".xml", ".g2o") + + for data_file_path in glob.glob(data_path, recursive=True): + for ext in extensions: + if (name + ext) == osp.basename(data_file_path): + return data_file_path From 597026f58ab9541868de831144bf0e1a630cc028 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 17:03:41 -0400 Subject: [PATCH 36/48] Undo Point operator overloading and instead update NavState print --- gtsam/geometry/Point2.cpp | 6 ------ gtsam/geometry/Point2.h | 2 -- gtsam/geometry/Point3.cpp | 6 ------ gtsam/geometry/Point3.h | 2 -- gtsam/navigation/NavState.cpp | 4 ++-- 5 files changed, 2 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 8f2b6c3e2..d8060cfcf 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -23,12 +23,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point2& p) { - os << p.transpose(); - return os; -} - /* ************************************************************************* */ double norm2(const Point2& p, OptionalJacobian<1,2> H) { double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 312e65b3b..e6574fe41 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -26,8 +26,6 @@ namespace gtsam { /// it is now possible to just typedef Point2 to Vector2 typedef Vector2 Point2; -GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2 &p); - /// Distance of the point from the origin, with Jacobian GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index c976f8bcc..7a46f5988 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -22,12 +22,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point3& p) { - os << p.transpose(); - return os; -} - /* ************************************************************************* */ double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index df0feff4f..19e328022 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -33,8 +33,6 @@ namespace gtsam { /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; -GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3 &p); - // Convenience typedef typedef std::pair Point3Pair; GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 1d191416f..7d4797132 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -89,8 +89,8 @@ Matrix7 NavState::matrix() const { //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const NavState& state) { os << "R: " << state.attitude() << "\n"; - os << "p: " << state.position() << "\n"; - os << "v: " << Point3(state.velocity()); + os << "p: " << state.position().transpose() << "\n"; + os << "v: " << state.velocity().transpose(); return os; } From 683e8f1f92d99199028d8bd824efea9584d8ccf7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Aug 2020 17:16:32 -0400 Subject: [PATCH 37/48] check for null pointer in general --- gtsam/nonlinear/NonlinearFactorGraph.cpp | 51 ++++++++++++++---------- 1 file changed, 29 insertions(+), 22 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 5f6cdcc98..0b876f376 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -205,42 +205,49 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections for(size_t i = 0; i < size(); ++i) { const NonlinearFactor::shared_ptr& factor = at(i); - if(formatting.plotFactorPoints) { + // If null pointer, move on to the next + if (!factor) { + continue; + } + + if (formatting.plotFactorPoints) { const KeyVector& keys = factor->keys(); - if (formatting.binaryEdges && keys.size()==2) { - stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; + if (formatting.binaryEdges && keys.size() == 2) { + stm << " var" << keys[0] << "--" + << "var" << keys[1] << ";\n"; } else { // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { - map::const_iterator pos = formatting.factorPositions.find(i); - if(pos != formatting.factorPositions.end()) - stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," - << formatting.scale*(pos->second.y() - minY) << "!\""; + map::const_iterator pos = + formatting.factorPositions.find(i); + if (pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale * (pos->second.x() - minX) + << "," << formatting.scale * (pos->second.y() - minY) + << "!\""; } stm << "];\n"; // Make factor-variable connections - if(formatting.connectKeysToFactor && factor) { - for(Key key: *factor) { - stm << " var" << key << "--" << "factor" << i << ";\n"; + if (formatting.connectKeysToFactor && factor) { + for (Key key : *factor) { + stm << " var" << key << "--" + << "factor" << i << ";\n"; } } } - } - else { - if(factor) { - Key k; - bool firstTime = true; - for(Key key: *this->at(i)) { - if(firstTime) { - k = key; - firstTime = false; - continue; - } - stm << " var" << key << "--" << "var" << k << ";\n"; + } else { + Key k; + bool firstTime = true; + for (Key key : *this->at(i)) { + if (firstTime) { k = key; + firstTime = false; + continue; } + stm << " var" << key << "--" + << "var" << k << ";\n"; + k = key; } } } From 0c62a9528deeee7eedae43250e9f0328c742c240 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 07:45:50 -0700 Subject: [PATCH 38/48] fixed issue that pos and vel are in nav frame --- gtsam/navigation/NavState.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7d4797132..6a196cb75 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -228,18 +228,27 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, dP(xi) -= (0.5 * dt2) * omega_cross2_t; dV(xi) -= dt * omega_cross2_t; } + + // NOTE(tim): position and velocity changes dP and dV are in nav frame; need + // to put them into local frame for subsequent retract + dP(xi) = nRb.unrotate(dP(xi)); + dV(xi) = nRb.unrotate(dV(xi)); + + if (H) { H->setZero(); const Matrix3 Omega = skewSymmetric(omega); const Matrix3 D_cross_state = Omega * R(); H->setZero(); D_R_R(H) << D_dP_R; - D_t_v(H) << (-dt2) * D_cross_state; - D_v_v(H) << (-2.0 * dt) * D_cross_state; + D_t_v(H) << nRb.matrix().transpose() * (-dt2) * D_cross_state; + D_t_R(H) << skewSymmetric(dP(xi)); + D_v_v(H) << nRb.matrix().transpose() * (-2.0 * dt) * D_cross_state; + D_v_R(H) << skewSymmetric(dV(xi)); if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= dt * D_cross2_state; + D_t_t(H) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state; } } return xi; From cb6573ad1fa0484034614e40b51336857a3d2aa2 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 13:01:52 -0700 Subject: [PATCH 39/48] added another coriolis unit test --- gtsam/navigation/tests/testNavState.cpp | 43 +++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 48dd3bc3e..95fd31c21 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) { EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); } +TEST(NavState, Coriolis3) { + /** Consider a massless plate with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * second order Coriolis corrections are as expected. + */ + + // Get true first and second order coriolis accelerations + double dt = 2.0, dt2 = dt * dt; + Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0); + Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v), + n_aCorr2 = -n_omega.cross(n_omega.cross(n_t)); + Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0), + bRn = nRb.inverse(); + + // Get expected first and second order corrections in the nav frame + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), + n_dV1e = dt * n_aCorr1, + n_dV2e = dt * (n_aCorr1 + n_aCorr2); + + // Get expected first and second order corrections in the body frame + Vector3 dRe = -dt * (bRn * n_omega), + b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e, + b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e; + + // Get actual first and scond order corrections in body frame + NavState kState2(nRb, n_t, n_v); + Vector9 dXi1a = kState2.coriolis(dt, n_omega, false), + dXi2a = kState2.coriolis(dt, n_omega, true); + Vector3 dRa = NavState::dR(dXi1a), + b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a), + b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a); + + EXPECT(assert_equal(dRe, dRa)); + EXPECT(assert_equal(b_dP1e, b_dP1a)); + EXPECT(assert_equal(b_dV1e, b_dV1a)); + EXPECT(assert_equal(b_dP2e, b_dP2a)); + EXPECT(assert_equal(b_dV2e, b_dV2a)); + +} + /* ************************************************************************* */ TEST(NavState, CorrectPIM) { Vector9 xi; From c3bddf18168fa5c95c7bce42d794368f703b0982 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 13:07:51 -0700 Subject: [PATCH 40/48] added cached rotation bRn --- gtsam/navigation/NavState.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6a196cb75..fcb85d2b7 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -215,6 +215,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { TIE(nRb, n_t, n_v, *this); + Matrix3 bRn = nRb.matrix().transpose(); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); @@ -231,8 +232,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, // NOTE(tim): position and velocity changes dP and dV are in nav frame; need // to put them into local frame for subsequent retract - dP(xi) = nRb.unrotate(dP(xi)); - dV(xi) = nRb.unrotate(dV(xi)); + dP(xi) = bRn * dP(xi); + dV(xi) = bRn * dV(xi); if (H) { @@ -241,14 +242,14 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, const Matrix3 D_cross_state = Omega * R(); H->setZero(); D_R_R(H) << D_dP_R; - D_t_v(H) << nRb.matrix().transpose() * (-dt2) * D_cross_state; + D_t_v(H) << bRn * (-dt2) * D_cross_state; D_t_R(H) << skewSymmetric(dP(xi)); - D_v_v(H) << nRb.matrix().transpose() * (-2.0 * dt) * D_cross_state; + D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state; D_v_R(H) << skewSymmetric(dV(xi)); if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state; + D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= bRn * dt * D_cross2_state; } } return xi; From 688292f114d04f8b519c6b9681b2bb690bb773d7 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Wed, 2 Sep 2020 07:59:19 -0700 Subject: [PATCH 41/48] cleaned up notation --- gtsam/navigation/NavState.cpp | 39 +++++++++++++++++------------------ 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index fcb85d2b7..0181ea45f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -215,41 +215,40 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { TIE(nRb, n_t, n_v, *this); - Matrix3 bRn = nRb.matrix().transpose(); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); - Vector9 xi; - Matrix3 D_dP_R; - dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); - dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper - dV(xi) << ((-2.0 * dt) * omega_cross_vel); + // Get perturbations in nav frame + Vector9 n_xi, xi; + Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; + dR(n_xi) << ((-dt) * omega); + dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper + dV(n_xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); - dP(xi) -= (0.5 * dt2) * omega_cross2_t; - dV(xi) -= dt * omega_cross2_t; + dP(n_xi) -= (0.5 * dt2) * omega_cross2_t; + dV(n_xi) -= dt * omega_cross2_t; } - // NOTE(tim): position and velocity changes dP and dV are in nav frame; need - // to put them into local frame for subsequent retract - dP(xi) = bRn * dP(xi); - dV(xi) = bRn * dV(xi); - + // Transform n_xi into the body frame + xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), + nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0), + nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0); if (H) { H->setZero(); const Matrix3 Omega = skewSymmetric(omega); const Matrix3 D_cross_state = Omega * R(); H->setZero(); - D_R_R(H) << D_dP_R; - D_t_v(H) << bRn * (-dt2) * D_cross_state; - D_t_R(H) << skewSymmetric(dP(xi)); - D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state; - D_v_R(H) << skewSymmetric(dV(xi)); + D_R_R(H) << D_dR_R; + D_t_v(H) << D_body_nav * (-dt2) * D_cross_state; + D_t_R(H) << D_dP_R; + D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state; + D_v_R(H) << D_dV_R; if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= bRn * dt * D_cross2_state; + D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= D_body_nav * dt * D_cross2_state; } } return xi; From 335b57ca363da3b4a1ca9c9a24c59a6d5b5fc339 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Wed, 2 Sep 2020 08:08:36 -0700 Subject: [PATCH 42/48] fixed typo --- gtsam/navigation/tests/testNavState.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 95fd31c21..5bafe4392 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -193,7 +193,7 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless plate with an attached nav frame at + /** Consider a massless planet with an attached nav frame at * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and From e95840b1b5cb318f2fdf3a45112d032f28e92379 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Sep 2020 00:33:38 -0400 Subject: [PATCH 43/48] new function readBal which returns the SfmData object, allowing for single line calling --- gtsam/slam/dataset.cpp | 7 +++++++ gtsam/slam/dataset.h | 8 ++++++++ 2 files changed, 15 insertions(+) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4bd7bc7e2..4506bcdb2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1129,6 +1129,13 @@ bool readBAL(const string &filename, SfmData &data) { return true; } +/* ************************************************************************* */ +SfmData readBAL(const string &filename) { + SfmData data; + readBAL(filename, data); + return data; +} + /* ************************************************************************* */ bool writeBAL(const string &filename, SfmData &data) { // Open the output file diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7ceca00f4..0bcdd7e8e 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -277,6 +277,14 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data); */ GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data); +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data + * as a SfmData structure. Mainly used by wrapped code. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ +GTSAM_EXPORT SfmData readBAL(const std::string& filename); + /** * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a * SfmData structure From f533729d3eaf9f0a44323d655d694af2c1f9ea51 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Sep 2020 00:43:44 -0400 Subject: [PATCH 44/48] follow Google Style for function naming --- gtsam/slam/dataset.cpp | 2 +- gtsam/slam/dataset.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4506bcdb2..270dbeb95 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1130,7 +1130,7 @@ bool readBAL(const string &filename, SfmData &data) { } /* ************************************************************************* */ -SfmData readBAL(const string &filename) { +SfmData readBal(const string &filename) { SfmData data; readBAL(filename, data); return data; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 0bcdd7e8e..2e03c92d8 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -283,7 +283,7 @@ GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data); * @param filename The name of the BAL file. * @return SfM structure where the data is stored. */ -GTSAM_EXPORT SfmData readBAL(const std::string& filename); +GTSAM_EXPORT SfmData readBal(const std::string& filename); /** * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a From 268c3609a85431f200c787c5c0601dfb812dfea4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Sep 2020 12:00:04 -0400 Subject: [PATCH 45/48] update minimum Boost version required --- CMakeLists.txt | 6 +++--- INSTALL.md | 2 +- README.md | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dce69903..aa6082cb3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,7 +166,7 @@ if(MSVC AND BUILD_SHARED_LIBS) endif() # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.43) +set(BOOST_FIND_MINIMUM_VERSION 1.58) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) @@ -174,7 +174,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") + message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) @@ -490,7 +490,7 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION # Deb-package specific cpack set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") ############################################################################### diff --git a/INSTALL.md b/INSTALL.md index b8f73f153..cf66766a1 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,7 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.58 or greater (install through Linux repositories or MacPorts) - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher diff --git a/README.md b/README.md index 3982f5585..cc8af7248 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ $ make install Prerequisites: -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. From e43f78bfe8e7f34d5bac95b023a07b25421799cc Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 6 Sep 2020 14:31:48 -0400 Subject: [PATCH 46/48] Wrap DSFMap so SFM can use them --- gtsam/base/DSFMap.h | 8 +++++ gtsam/gtsam.i | 58 +++++++++++++++++++++++++++++++--- python/CMakeLists.txt | 2 ++ python/gtsam/preamble.h | 1 + python/gtsam/specializations.h | 2 ++ 5 files changed, 66 insertions(+), 5 deletions(-) diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index 1d6bfdefa..6d0fbca20 100644 --- a/gtsam/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -120,4 +120,12 @@ class IndexPair : public std::pair { inline size_t i() const { return first; }; inline size_t j() const { return second; }; }; + +typedef std::vector IndexPairVector; +typedef std::set IndexPairSet; + +inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); } + +typedef std::map IndexPairSetMap; +typedef DSFMap DSFMapIndexPair; } // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 33d8c11fe..75f11b449 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -259,11 +259,59 @@ class IndexPair { size_t j() const; }; -template -class DSFMap { - DSFMap(); - KEY find(const KEY& key) const; - void merge(const KEY& x, const KEY& y); +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; + +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; + +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; + +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); + +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); }; #include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e15e571f5..07e2d51ac 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,6 +30,8 @@ set(ignore gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet + gtsam::IndexPairSetMap + gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 9f3032dd0..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3b60e42cb..dbb95c9d6 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -9,3 +9,5 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); \ No newline at end of file From 788a8771c410498e85c2dbe5b3f78b7fac471ae2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 6 Sep 2020 14:34:30 -0400 Subject: [PATCH 47/48] Add the unit test for DSF in Python --- python/gtsam/tests/test_dsf_map.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py index 73ddcb050..e36657178 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_dsf_map.py @@ -35,6 +35,20 @@ class TestDSFMap(GtsamTestCase): dsf.merge(pair1, pair2) self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) + def test_sets(self): + from gtsam import IndexPair + dsf = gtsam.DSFMapIndexPair() + dsf.merge(IndexPair(0, 1), IndexPair(1,2)) + dsf.merge(IndexPair(0, 1), IndexPair(3,4)) + dsf.merge(IndexPair(4,5), IndexPair(6,8)) + sets = dsf.sets() + + for i in sets: + s = sets[i] + for val in gtsam.IndexPairSetAsArray(s): + val.i() + val.j() + if __name__ == '__main__': unittest.main() From 00fbea77ddc8697f0384058304acd000dfc0a2e4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 6 Sep 2020 14:43:10 -0400 Subject: [PATCH 48/48] Fix include issue --- gtsam/base/DSFMap.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index 6d0fbca20..3a9d21aad 100644 --- a/gtsam/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -21,6 +21,7 @@ #include // Provides size_t #include #include +#include namespace gtsam {