Merge pull request #903 from borglab/feature/python-examples
Varun Agrawal
GitHub
commit
c56579c61d
|
|
@ -9,15 +9,17 @@ CustomFactor demo that simulates a 1-D sensor fusion task.
|
||||||
Author: Fan Jiang, Frank Dellaert
|
Author: Fan Jiang, Frank Dellaert
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
from functools import partial
|
||||||
|
from typing import List, Optional
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from typing import List, Optional
|
I = np.eye(1)
|
||||||
from functools import partial
|
|
||||||
|
|
||||||
|
|
||||||
def simulate_car():
|
def simulate_car() -> List[float]:
|
||||||
# Simulate a car for one second
|
"""Simulate a car for one second"""
|
||||||
x0 = 0
|
x0 = 0
|
||||||
dt = 0.25 # 4 Hz, typical GPS
|
dt = 0.25 # 4 Hz, typical GPS
|
||||||
v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast
|
v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast
|
||||||
|
|
@ -26,46 +28,9 @@ def simulate_car():
|
||||||
return x
|
return x
|
||||||
|
|
||||||
|
|
||||||
x = simulate_car()
|
def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor,
|
||||||
print(f"Simulated car trajectory: {x}")
|
values: gtsam.Values,
|
||||||
|
jacobians: Optional[List[np.ndarray]]) -> float:
|
||||||
# %%
|
|
||||||
add_noise = True # set this to False to run with "perfect" measurements
|
|
||||||
|
|
||||||
# GPS measurements
|
|
||||||
sigma_gps = 3.0 # assume GPS is +/- 3m
|
|
||||||
g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
|
|
||||||
for k in range(5)]
|
|
||||||
|
|
||||||
# Odometry measurements
|
|
||||||
sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz
|
|
||||||
o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
|
|
||||||
for k in range(4)]
|
|
||||||
|
|
||||||
# Landmark measurements:
|
|
||||||
sigma_lm = 1 # assume landmark measurement is accurate up to 1m
|
|
||||||
|
|
||||||
# Assume first landmark is at x=5, we measure it at time k=0
|
|
||||||
lm_0 = 5.0
|
|
||||||
z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
|
|
||||||
|
|
||||||
# Assume other landmark is at x=28, we measure it at time k=3
|
|
||||||
lm_3 = 28.0
|
|
||||||
z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
|
|
||||||
|
|
||||||
unknown = [gtsam.symbol('x', k) for k in range(5)]
|
|
||||||
|
|
||||||
print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
|
|
||||||
|
|
||||||
# We now can use nonlinear factor graphs
|
|
||||||
factor_graph = gtsam.NonlinearFactorGraph()
|
|
||||||
|
|
||||||
# Add factors for GPS measurements
|
|
||||||
I = np.eye(1)
|
|
||||||
gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
|
|
||||||
|
|
||||||
|
|
||||||
def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
|
|
||||||
"""GPS Factor error function
|
"""GPS Factor error function
|
||||||
:param measurement: GPS measurement, to be filled with `partial`
|
:param measurement: GPS measurement, to be filled with `partial`
|
||||||
:param this: gtsam.CustomFactor handle
|
:param this: gtsam.CustomFactor handle
|
||||||
|
|
@ -82,36 +47,9 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobia
|
||||||
return error
|
return error
|
||||||
|
|
||||||
|
|
||||||
# Add the GPS factors
|
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor,
|
||||||
for k in range(5):
|
values: gtsam.Values,
|
||||||
gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]])))
|
jacobians: Optional[List[np.ndarray]]) -> float:
|
||||||
factor_graph.add(gf)
|
|
||||||
|
|
||||||
# New Values container
|
|
||||||
v = gtsam.Values()
|
|
||||||
|
|
||||||
# Add initial estimates to the Values container
|
|
||||||
for i in range(5):
|
|
||||||
v.insert(unknown[i], np.array([0.0]))
|
|
||||||
|
|
||||||
# Initialize optimizer
|
|
||||||
params = gtsam.GaussNewtonParams()
|
|
||||||
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
|
|
||||||
|
|
||||||
# Optimize the factor graph
|
|
||||||
result = optimizer.optimize()
|
|
||||||
|
|
||||||
# calculate the error from ground truth
|
|
||||||
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
|
|
||||||
|
|
||||||
print("Result with only GPS")
|
|
||||||
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
|
|
||||||
|
|
||||||
# Adding odometry will improve things a lot
|
|
||||||
odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
|
|
||||||
|
|
||||||
|
|
||||||
def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
|
|
||||||
"""Odometry Factor error function
|
"""Odometry Factor error function
|
||||||
:param measurement: Odometry measurement, to be filled with `partial`
|
:param measurement: Odometry measurement, to be filled with `partial`
|
||||||
:param this: gtsam.CustomFactor handle
|
:param this: gtsam.CustomFactor handle
|
||||||
|
|
@ -130,25 +68,9 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobi
|
||||||
return error
|
return error
|
||||||
|
|
||||||
|
|
||||||
for k in range(4):
|
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor,
|
||||||
odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]])))
|
values: gtsam.Values,
|
||||||
factor_graph.add(odof)
|
jacobians: Optional[List[np.ndarray]]) -> float:
|
||||||
|
|
||||||
params = gtsam.GaussNewtonParams()
|
|
||||||
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
|
|
||||||
|
|
||||||
result = optimizer.optimize()
|
|
||||||
|
|
||||||
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
|
|
||||||
|
|
||||||
print("Result with GPS+Odometry")
|
|
||||||
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
|
|
||||||
|
|
||||||
# This is great, but GPS noise is still apparent, so now we add the two landmarks
|
|
||||||
lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
|
|
||||||
|
|
||||||
|
|
||||||
def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
|
|
||||||
"""Landmark Factor error function
|
"""Landmark Factor error function
|
||||||
:param measurement: Landmark measurement, to be filled with `partial`
|
:param measurement: Landmark measurement, to be filled with `partial`
|
||||||
:param this: gtsam.CustomFactor handle
|
:param this: gtsam.CustomFactor handle
|
||||||
|
|
@ -165,15 +87,120 @@ def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobian
|
||||||
return error
|
return error
|
||||||
|
|
||||||
|
|
||||||
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0]))))
|
def main():
|
||||||
factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3]))))
|
"""Main runner."""
|
||||||
|
|
||||||
params = gtsam.GaussNewtonParams()
|
x = simulate_car()
|
||||||
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
|
print(f"Simulated car trajectory: {x}")
|
||||||
|
|
||||||
result = optimizer.optimize()
|
add_noise = True # set this to False to run with "perfect" measurements
|
||||||
|
|
||||||
error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
|
# GPS measurements
|
||||||
|
sigma_gps = 3.0 # assume GPS is +/- 3m
|
||||||
|
g = [
|
||||||
|
x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
|
||||||
|
for k in range(5)
|
||||||
|
]
|
||||||
|
|
||||||
print("Result with GPS+Odometry+Landmark")
|
# Odometry measurements
|
||||||
print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
|
sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz
|
||||||
|
o = [
|
||||||
|
x[k + 1] - x[k] +
|
||||||
|
(np.random.normal(scale=sigma_odo) if add_noise else 0)
|
||||||
|
for k in range(4)
|
||||||
|
]
|
||||||
|
|
||||||
|
# Landmark measurements:
|
||||||
|
sigma_lm = 1 # assume landmark measurement is accurate up to 1m
|
||||||
|
|
||||||
|
# Assume first landmark is at x=5, we measure it at time k=0
|
||||||
|
lm_0 = 5.0
|
||||||
|
z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
|
||||||
|
|
||||||
|
# Assume other landmark is at x=28, we measure it at time k=3
|
||||||
|
lm_3 = 28.0
|
||||||
|
z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
|
||||||
|
|
||||||
|
unknown = [gtsam.symbol('x', k) for k in range(5)]
|
||||||
|
|
||||||
|
print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
|
||||||
|
|
||||||
|
# We now can use nonlinear factor graphs
|
||||||
|
factor_graph = gtsam.NonlinearFactorGraph()
|
||||||
|
|
||||||
|
# Add factors for GPS measurements
|
||||||
|
gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
|
||||||
|
|
||||||
|
# Add the GPS factors
|
||||||
|
for k in range(5):
|
||||||
|
gf = gtsam.CustomFactor(gps_model, [unknown[k]],
|
||||||
|
partial(error_gps, np.array([g[k]])))
|
||||||
|
factor_graph.add(gf)
|
||||||
|
|
||||||
|
# New Values container
|
||||||
|
v = gtsam.Values()
|
||||||
|
|
||||||
|
# Add initial estimates to the Values container
|
||||||
|
for i in range(5):
|
||||||
|
v.insert(unknown[i], np.array([0.0]))
|
||||||
|
|
||||||
|
# Initialize optimizer
|
||||||
|
params = gtsam.GaussNewtonParams()
|
||||||
|
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
|
||||||
|
|
||||||
|
# Optimize the factor graph
|
||||||
|
result = optimizer.optimize()
|
||||||
|
|
||||||
|
# calculate the error from ground truth
|
||||||
|
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
|
||||||
|
for k in range(5)])
|
||||||
|
|
||||||
|
print("Result with only GPS")
|
||||||
|
print(result, np.round(error, 2),
|
||||||
|
f"\nJ(X)={0.5 * np.sum(np.square(error))}")
|
||||||
|
|
||||||
|
# Adding odometry will improve things a lot
|
||||||
|
odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
|
||||||
|
|
||||||
|
for k in range(4):
|
||||||
|
odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]],
|
||||||
|
partial(error_odom, np.array([o[k]])))
|
||||||
|
factor_graph.add(odof)
|
||||||
|
|
||||||
|
params = gtsam.GaussNewtonParams()
|
||||||
|
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
|
||||||
|
|
||||||
|
result = optimizer.optimize()
|
||||||
|
|
||||||
|
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
|
||||||
|
for k in range(5)])
|
||||||
|
|
||||||
|
print("Result with GPS+Odometry")
|
||||||
|
print(result, np.round(error, 2),
|
||||||
|
f"\nJ(X)={0.5 * np.sum(np.square(error))}")
|
||||||
|
|
||||||
|
# This is great, but GPS noise is still apparent, so now we add the two landmarks
|
||||||
|
lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
|
||||||
|
|
||||||
|
factor_graph.add(
|
||||||
|
gtsam.CustomFactor(lm_model, [unknown[0]],
|
||||||
|
partial(error_lm, np.array([lm_0 + z_0]))))
|
||||||
|
factor_graph.add(
|
||||||
|
gtsam.CustomFactor(lm_model, [unknown[3]],
|
||||||
|
partial(error_lm, np.array([lm_3 + z_3]))))
|
||||||
|
|
||||||
|
params = gtsam.GaussNewtonParams()
|
||||||
|
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
|
||||||
|
|
||||||
|
result = optimizer.optimize()
|
||||||
|
|
||||||
|
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
|
||||||
|
for k in range(5)])
|
||||||
|
|
||||||
|
print("Result with GPS+Odometry+Landmark")
|
||||||
|
print(result, np.round(error, 2),
|
||||||
|
f"\nJ(X)={0.5 * np.sum(np.square(error))}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
|
|
@ -13,13 +13,8 @@ Author: Mandy Xie
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import gtsam.utils.plot as gtsam_plot
|
|
||||||
|
|
||||||
# ENU Origin is where the plane was in hold next to runway
|
# ENU Origin is where the plane was in hold next to runway
|
||||||
lat0 = 33.86998
|
lat0 = 33.86998
|
||||||
lon0 = -84.30626
|
lon0 = -84.30626
|
||||||
|
|
@ -29,28 +24,34 @@ h0 = 274
|
||||||
GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||||
PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)
|
PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)
|
||||||
|
|
||||||
# Create an empty nonlinear factor graph
|
|
||||||
graph = gtsam.NonlinearFactorGraph()
|
|
||||||
|
|
||||||
# Add a prior on the first point, setting it to the origin
|
def main():
|
||||||
# A prior factor consists of a mean and a noise model (covariance matrix)
|
"""Main runner."""
|
||||||
priorMean = gtsam.Pose3() # prior at origin
|
# Create an empty nonlinear factor graph
|
||||||
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
|
|
||||||
# Add GPS factors
|
# Add a prior on the first point, setting it to the origin
|
||||||
gps = gtsam.Point3(lat0, lon0, h0)
|
# A prior factor consists of a mean and a noise model (covariance matrix)
|
||||||
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
|
priorMean = gtsam.Pose3() # prior at origin
|
||||||
print("\nFactor Graph:\n{}".format(graph))
|
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
|
||||||
|
|
||||||
# Create the data structure to hold the initialEstimate estimate to the solution
|
# Add GPS factors
|
||||||
# For illustrative purposes, these have been deliberately set to incorrect values
|
gps = gtsam.Point3(lat0, lon0, h0)
|
||||||
initial = gtsam.Values()
|
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
|
||||||
initial.insert(1, gtsam.Pose3())
|
print("\nFactor Graph:\n{}".format(graph))
|
||||||
print("\nInitial Estimate:\n{}".format(initial))
|
|
||||||
|
|
||||||
# optimize using Levenberg-Marquardt optimization
|
# Create the data structure to hold the initialEstimate estimate to the solution
|
||||||
params = gtsam.LevenbergMarquardtParams()
|
# For illustrative purposes, these have been deliberately set to incorrect values
|
||||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
initial = gtsam.Values()
|
||||||
result = optimizer.optimize()
|
initial.insert(1, gtsam.Pose3())
|
||||||
print("\nFinal Result:\n{}".format(result))
|
print("\nInitial Estimate:\n{}".format(initial))
|
||||||
|
|
||||||
|
# optimize using Levenberg-Marquardt optimization
|
||||||
|
params = gtsam.LevenbergMarquardtParams()
|
||||||
|
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||||
|
result = optimizer.optimize()
|
||||||
|
print("\nFinal Result:\n{}".format(result))
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
|
|
@ -13,57 +13,60 @@ Author: Frank Dellaert
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import gtsam.utils.plot as gtsam_plot
|
import gtsam.utils.plot as gtsam_plot
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
# Create noise models
|
# Create noise models
|
||||||
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||||
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||||
|
|
||||||
# Create an empty nonlinear factor graph
|
|
||||||
graph = gtsam.NonlinearFactorGraph()
|
|
||||||
|
|
||||||
# Add a prior on the first pose, setting it to the origin
|
def main():
|
||||||
# A prior factor consists of a mean and a noise model (covariance matrix)
|
"""Main runner"""
|
||||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
# Create an empty nonlinear factor graph
|
||||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
|
|
||||||
# Add odometry factors
|
# Add a prior on the first pose, setting it to the origin
|
||||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
# A prior factor consists of a mean and a noise model (covariance matrix)
|
||||||
# For simplicity, we will use the same noise model for each odometry factor
|
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||||
# Create odometry (Between) factors between consecutive poses
|
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
|
||||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
|
|
||||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
|
|
||||||
print("\nFactor Graph:\n{}".format(graph))
|
|
||||||
|
|
||||||
# Create the data structure to hold the initialEstimate estimate to the solution
|
# Add odometry factors
|
||||||
# For illustrative purposes, these have been deliberately set to incorrect values
|
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||||
initial = gtsam.Values()
|
# For simplicity, we will use the same noise model for each odometry factor
|
||||||
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
# Create odometry (Between) factors between consecutive poses
|
||||||
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
|
||||||
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
|
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
|
||||||
print("\nInitial Estimate:\n{}".format(initial))
|
print("\nFactor Graph:\n{}".format(graph))
|
||||||
|
|
||||||
# optimize using Levenberg-Marquardt optimization
|
# Create the data structure to hold the initialEstimate estimate to the solution
|
||||||
params = gtsam.LevenbergMarquardtParams()
|
# For illustrative purposes, these have been deliberately set to incorrect values
|
||||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
initial = gtsam.Values()
|
||||||
result = optimizer.optimize()
|
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||||
print("\nFinal Result:\n{}".format(result))
|
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||||
|
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||||
|
print("\nInitial Estimate:\n{}".format(initial))
|
||||||
|
|
||||||
# 5. Calculate and print marginal covariances for all variables
|
# optimize using Levenberg-Marquardt optimization
|
||||||
marginals = gtsam.Marginals(graph, result)
|
params = gtsam.LevenbergMarquardtParams()
|
||||||
for i in range(1, 4):
|
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||||
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
|
result = optimizer.optimize()
|
||||||
|
print("\nFinal Result:\n{}".format(result))
|
||||||
fig = plt.figure(0)
|
|
||||||
for i in range(1, 4):
|
|
||||||
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
|
|
||||||
plt.axis('equal')
|
|
||||||
plt.show()
|
|
||||||
|
|
||||||
|
# 5. Calculate and print marginal covariances for all variables
|
||||||
|
marginals = gtsam.Marginals(graph, result)
|
||||||
|
for i in range(1, 4):
|
||||||
|
print("X{} covariance:\n{}\n".format(i,
|
||||||
|
marginals.marginalCovariance(i)))
|
||||||
|
|
||||||
|
for i in range(1, 4):
|
||||||
|
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
|
||||||
|
marginals.marginalCovariance(i))
|
||||||
|
plt.axis('equal')
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
|
|
@ -13,69 +13,85 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
from gtsam.symbol_shorthand import X, L
|
import numpy as np
|
||||||
|
from gtsam.symbol_shorthand import L, X
|
||||||
|
|
||||||
# Create noise models
|
# Create noise models
|
||||||
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||||
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||||
MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
|
MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
|
||||||
|
|
||||||
# Create an empty nonlinear factor graph
|
|
||||||
graph = gtsam.NonlinearFactorGraph()
|
|
||||||
|
|
||||||
# Create the keys corresponding to unknown variables in the factor graph
|
def main():
|
||||||
X1 = X(1)
|
"""Main runner"""
|
||||||
X2 = X(2)
|
|
||||||
X3 = X(3)
|
|
||||||
L1 = L(4)
|
|
||||||
L2 = L(5)
|
|
||||||
|
|
||||||
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
|
# Create an empty nonlinear factor graph
|
||||||
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
|
|
||||||
# Add odometry factors between X1,X2 and X2,X3, respectively
|
# Create the keys corresponding to unknown variables in the factor graph
|
||||||
graph.add(gtsam.BetweenFactorPose2(
|
X1 = X(1)
|
||||||
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
X2 = X(2)
|
||||||
graph.add(gtsam.BetweenFactorPose2(
|
X3 = X(3)
|
||||||
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
L1 = L(4)
|
||||||
|
L2 = L(5)
|
||||||
|
|
||||||
# Add Range-Bearing measurements to two different landmarks L1 and L2
|
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
|
||||||
graph.add(gtsam.BearingRangeFactor2D(
|
graph.add(
|
||||||
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
|
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
|
||||||
graph.add(gtsam.BearingRangeFactor2D(
|
|
||||||
X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
|
||||||
graph.add(gtsam.BearingRangeFactor2D(
|
|
||||||
X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
|
||||||
|
|
||||||
# Print graph
|
# Add odometry factors between X1,X2 and X2,X3, respectively
|
||||||
print("Factor Graph:\n{}".format(graph))
|
graph.add(
|
||||||
|
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
|
||||||
|
ODOMETRY_NOISE))
|
||||||
|
graph.add(
|
||||||
|
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
|
||||||
|
ODOMETRY_NOISE))
|
||||||
|
|
||||||
# Create (deliberately inaccurate) initial estimate
|
# Add Range-Bearing measurements to two different landmarks L1 and L2
|
||||||
initial_estimate = gtsam.Values()
|
graph.add(
|
||||||
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
|
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
|
||||||
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
|
np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
|
||||||
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
|
graph.add(
|
||||||
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
|
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
|
||||||
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
|
MEASUREMENT_NOISE))
|
||||||
|
graph.add(
|
||||||
|
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
|
||||||
|
MEASUREMENT_NOISE))
|
||||||
|
|
||||||
# Print
|
# Print graph
|
||||||
print("Initial Estimate:\n{}".format(initial_estimate))
|
print("Factor Graph:\n{}".format(graph))
|
||||||
|
|
||||||
# Optimize using Levenberg-Marquardt optimization. The optimizer
|
# Create (deliberately inaccurate) initial estimate
|
||||||
# accepts an optional set of configuration parameters, controlling
|
initial_estimate = gtsam.Values()
|
||||||
# things like convergence criteria, the type of linear system solver
|
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
|
||||||
# to use, and the amount of information displayed during optimization.
|
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
|
||||||
# Here we will use the default set of parameters. See the
|
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
|
||||||
# documentation for the full set of parameters.
|
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
|
||||||
params = gtsam.LevenbergMarquardtParams()
|
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
|
||||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
|
|
||||||
result = optimizer.optimize()
|
|
||||||
print("\nFinal Result:\n{}".format(result))
|
|
||||||
|
|
||||||
# Calculate and print marginal covariances for all variables
|
# Print
|
||||||
marginals = gtsam.Marginals(graph, result)
|
print("Initial Estimate:\n{}".format(initial_estimate))
|
||||||
for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
|
|
||||||
print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))
|
# Optimize using Levenberg-Marquardt optimization. The optimizer
|
||||||
|
# accepts an optional set of configuration parameters, controlling
|
||||||
|
# things like convergence criteria, the type of linear system solver
|
||||||
|
# to use, and the amount of information displayed during optimization.
|
||||||
|
# Here we will use the default set of parameters. See the
|
||||||
|
# documentation for the full set of parameters.
|
||||||
|
params = gtsam.LevenbergMarquardtParams()
|
||||||
|
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
|
||||||
|
params)
|
||||||
|
result = optimizer.optimize()
|
||||||
|
print("\nFinal Result:\n{}".format(result))
|
||||||
|
|
||||||
|
# Calculate and print marginal covariances for all variables
|
||||||
|
marginals = gtsam.Marginals(graph, result)
|
||||||
|
for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
|
||||||
|
(L2, "L2")]:
|
||||||
|
print("{} covariance:\n{}\n".format(s,
|
||||||
|
marginals.marginalCovariance(key)))
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
|
|
@ -15,82 +15,88 @@ from __future__ import print_function
|
||||||
|
|
||||||
import math
|
import math
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import gtsam.utils.plot as gtsam_plot
|
import gtsam.utils.plot as gtsam_plot
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
|
||||||
def vector3(x, y, z):
|
def main():
|
||||||
"""Create 3d double numpy array."""
|
"""Main runner."""
|
||||||
return np.array([x, y, z], dtype=float)
|
# Create noise models
|
||||||
|
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1))
|
||||||
|
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(
|
||||||
|
gtsam.Point3(0.2, 0.2, 0.1))
|
||||||
|
|
||||||
# Create noise models
|
# 1. Create a factor graph container and add factors to it
|
||||||
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
|
|
||||||
|
|
||||||
# 1. Create a factor graph container and add factors to it
|
# 2a. Add a prior on the first pose, setting it to the origin
|
||||||
graph = gtsam.NonlinearFactorGraph()
|
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
|
||||||
|
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
|
||||||
|
|
||||||
# 2a. Add a prior on the first pose, setting it to the origin
|
# 2b. Add odometry factors
|
||||||
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
|
# Create odometry (Between) factors between consecutive poses
|
||||||
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
|
graph.add(
|
||||||
|
gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
|
||||||
|
graph.add(
|
||||||
|
gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2),
|
||||||
|
ODOMETRY_NOISE))
|
||||||
|
graph.add(
|
||||||
|
gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2),
|
||||||
|
ODOMETRY_NOISE))
|
||||||
|
graph.add(
|
||||||
|
gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2),
|
||||||
|
ODOMETRY_NOISE))
|
||||||
|
|
||||||
# 2b. Add odometry factors
|
# 2c. Add the loop closure constraint
|
||||||
# Create odometry (Between) factors between consecutive poses
|
# This factor encodes the fact that we have returned to the same pose. In real
|
||||||
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
|
# systems, these constraints may be identified in many ways, such as appearance-based
|
||||||
graph.add(gtsam.BetweenFactorPose2(
|
# techniques with camera images. We will use another Between Factor to enforce this constraint:
|
||||||
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
graph.add(
|
||||||
graph.add(gtsam.BetweenFactorPose2(
|
gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2),
|
||||||
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
ODOMETRY_NOISE))
|
||||||
graph.add(gtsam.BetweenFactorPose2(
|
print("\nFactor Graph:\n{}".format(graph)) # print
|
||||||
4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
|
||||||
|
|
||||||
# 2c. Add the loop closure constraint
|
# 3. Create the data structure to hold the initial_estimate estimate to the
|
||||||
# This factor encodes the fact that we have returned to the same pose. In real
|
# solution. For illustrative purposes, these have been deliberately set to incorrect values
|
||||||
# systems, these constraints may be identified in many ways, such as appearance-based
|
initial_estimate = gtsam.Values()
|
||||||
# techniques with camera images. We will use another Between Factor to enforce this constraint:
|
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||||
graph.add(gtsam.BetweenFactorPose2(
|
initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||||
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
|
||||||
print("\nFactor Graph:\n{}".format(graph)) # print
|
initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
|
||||||
|
initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))
|
||||||
|
print("\nInitial Estimate:\n{}".format(initial_estimate)) # print
|
||||||
|
|
||||||
# 3. Create the data structure to hold the initial_estimate estimate to the
|
# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
|
||||||
# solution. For illustrative purposes, these have been deliberately set to incorrect values
|
# The optimizer accepts an optional set of configuration parameters,
|
||||||
initial_estimate = gtsam.Values()
|
# controlling things like convergence criteria, the type of linear
|
||||||
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
# system solver to use, and the amount of information displayed during
|
||||||
initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
# optimization. We will set a few parameters as a demonstration.
|
||||||
initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
|
parameters = gtsam.GaussNewtonParams()
|
||||||
initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
|
|
||||||
initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))
|
|
||||||
print("\nInitial Estimate:\n{}".format(initial_estimate)) # print
|
|
||||||
|
|
||||||
# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
|
# Stop iterating once the change in error between steps is less than this value
|
||||||
# The optimizer accepts an optional set of configuration parameters,
|
parameters.setRelativeErrorTol(1e-5)
|
||||||
# controlling things like convergence criteria, the type of linear
|
# Do not perform more than N iteration steps
|
||||||
# system solver to use, and the amount of information displayed during
|
parameters.setMaxIterations(100)
|
||||||
# optimization. We will set a few parameters as a demonstration.
|
# Create the optimizer ...
|
||||||
parameters = gtsam.GaussNewtonParams()
|
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)
|
||||||
|
# ... and optimize
|
||||||
|
result = optimizer.optimize()
|
||||||
|
print("Final Result:\n{}".format(result))
|
||||||
|
|
||||||
# Stop iterating once the change in error between steps is less than this value
|
# 5. Calculate and print marginal covariances for all variables
|
||||||
parameters.setRelativeErrorTol(1e-5)
|
marginals = gtsam.Marginals(graph, result)
|
||||||
# Do not perform more than N iteration steps
|
for i in range(1, 6):
|
||||||
parameters.setMaxIterations(100)
|
print("X{} covariance:\n{}\n".format(i,
|
||||||
# Create the optimizer ...
|
marginals.marginalCovariance(i)))
|
||||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)
|
|
||||||
# ... and optimize
|
|
||||||
result = optimizer.optimize()
|
|
||||||
print("Final Result:\n{}".format(result))
|
|
||||||
|
|
||||||
# 5. Calculate and print marginal covariances for all variables
|
for i in range(1, 6):
|
||||||
marginals = gtsam.Marginals(graph, result)
|
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
|
||||||
for i in range(1, 6):
|
marginals.marginalCovariance(i))
|
||||||
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
|
|
||||||
|
|
||||||
fig = plt.figure(0)
|
plt.axis('equal')
|
||||||
for i in range(1, 6):
|
plt.show()
|
||||||
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
|
|
||||||
|
|
||||||
plt.axis('equal')
|
|
||||||
plt.show()
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
|
|
@ -12,77 +12,86 @@ and does the optimization. Output is written on a file, in g2o format
|
||||||
# pylint: disable=invalid-name, E1101
|
# pylint: disable=invalid-name, E1101
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
from gtsam.utils import plot
|
from gtsam.utils import plot
|
||||||
|
|
||||||
|
|
||||||
def vector3(x, y, z):
|
def main():
|
||||||
"""Create 3d double numpy array."""
|
"""Main runner."""
|
||||||
return np.array([x, y, z], dtype=float)
|
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description="A 2D Pose SLAM example that reads input from g2o, "
|
||||||
|
"converts it to a factor graph and does the optimization. "
|
||||||
|
"Output is written on a file, in g2o format")
|
||||||
|
parser.add_argument('-i', '--input', help='input file g2o format')
|
||||||
|
parser.add_argument(
|
||||||
|
'-o',
|
||||||
|
'--output',
|
||||||
|
help="the path to the output file with optimized graph")
|
||||||
|
parser.add_argument('-m',
|
||||||
|
'--maxiter',
|
||||||
|
type=int,
|
||||||
|
help="maximum number of iterations for optimizer")
|
||||||
|
parser.add_argument('-k',
|
||||||
|
'--kernel',
|
||||||
|
choices=['none', 'huber', 'tukey'],
|
||||||
|
default="none",
|
||||||
|
help="Type of kernel used")
|
||||||
|
parser.add_argument("-p",
|
||||||
|
"--plot",
|
||||||
|
action="store_true",
|
||||||
|
help="Flag to plot results")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
|
||||||
|
else args.input
|
||||||
|
|
||||||
|
maxIterations = 100 if args.maxiter is None else args.maxiter
|
||||||
|
|
||||||
|
is3D = False
|
||||||
|
|
||||||
|
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
||||||
|
|
||||||
|
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
|
||||||
|
|
||||||
|
# Add prior on the pose having index (key) = 0
|
||||||
|
priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8))
|
||||||
|
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
|
||||||
|
|
||||||
|
params = gtsam.GaussNewtonParams()
|
||||||
|
params.setVerbosity("Termination")
|
||||||
|
params.setMaxIterations(maxIterations)
|
||||||
|
# parameters.setRelativeErrorTol(1e-5)
|
||||||
|
# Create the optimizer ...
|
||||||
|
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
|
||||||
|
# ... and optimize
|
||||||
|
result = optimizer.optimize()
|
||||||
|
|
||||||
|
print("Optimization complete")
|
||||||
|
print("initial error = ", graph.error(initial))
|
||||||
|
print("final error = ", graph.error(result))
|
||||||
|
|
||||||
|
if args.output is None:
|
||||||
|
print("\nFactor Graph:\n{}".format(graph))
|
||||||
|
print("\nInitial Estimate:\n{}".format(initial))
|
||||||
|
print("Final Result:\n{}".format(result))
|
||||||
|
else:
|
||||||
|
outputFile = args.output
|
||||||
|
print("Writing results to file: ", outputFile)
|
||||||
|
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
|
||||||
|
gtsam.writeG2o(graphNoKernel, result, outputFile)
|
||||||
|
print("Done!")
|
||||||
|
|
||||||
|
if args.plot:
|
||||||
|
resultPoses = gtsam.utilities.extractPose2(result)
|
||||||
|
for i in range(resultPoses.shape[0]):
|
||||||
|
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
|
||||||
parser = argparse.ArgumentParser(
|
if __name__ == "__main__":
|
||||||
description="A 2D Pose SLAM example that reads input from g2o, "
|
main()
|
||||||
"converts it to a factor graph and does the optimization. "
|
|
||||||
"Output is written on a file, in g2o format")
|
|
||||||
parser.add_argument('-i', '--input', help='input file g2o format')
|
|
||||||
parser.add_argument('-o', '--output',
|
|
||||||
help="the path to the output file with optimized graph")
|
|
||||||
parser.add_argument('-m', '--maxiter', type=int,
|
|
||||||
help="maximum number of iterations for optimizer")
|
|
||||||
parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
|
|
||||||
default="none", help="Type of kernel used")
|
|
||||||
parser.add_argument("-p", "--plot", action="store_true",
|
|
||||||
help="Flag to plot results")
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
|
|
||||||
else args.input
|
|
||||||
|
|
||||||
maxIterations = 100 if args.maxiter is None else args.maxiter
|
|
||||||
|
|
||||||
is3D = False
|
|
||||||
|
|
||||||
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
|
||||||
|
|
||||||
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
|
|
||||||
|
|
||||||
# Add prior on the pose having index (key) = 0
|
|
||||||
priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
|
|
||||||
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
|
|
||||||
|
|
||||||
params = gtsam.GaussNewtonParams()
|
|
||||||
params.setVerbosity("Termination")
|
|
||||||
params.setMaxIterations(maxIterations)
|
|
||||||
# parameters.setRelativeErrorTol(1e-5)
|
|
||||||
# Create the optimizer ...
|
|
||||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
|
|
||||||
# ... and optimize
|
|
||||||
result = optimizer.optimize()
|
|
||||||
|
|
||||||
print("Optimization complete")
|
|
||||||
print("initial error = ", graph.error(initial))
|
|
||||||
print("final error = ", graph.error(result))
|
|
||||||
|
|
||||||
|
|
||||||
if args.output is None:
|
|
||||||
print("\nFactor Graph:\n{}".format(graph))
|
|
||||||
print("\nInitial Estimate:\n{}".format(initial))
|
|
||||||
print("Final Result:\n{}".format(result))
|
|
||||||
else:
|
|
||||||
outputFile = args.output
|
|
||||||
print("Writing results to file: ", outputFile)
|
|
||||||
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
|
|
||||||
gtsam.writeG2o(graphNoKernel, result, outputFile)
|
|
||||||
print ("Done!")
|
|
||||||
|
|
||||||
if args.plot:
|
|
||||||
resultPoses = gtsam.utilities.extractPose2(result)
|
|
||||||
for i in range(resultPoses.shape[0]):
|
|
||||||
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
|
|
||||||
plt.show()
|
|
||||||
|
|
|
||||||
|
|
@ -8,13 +8,14 @@
|
||||||
# pylint: disable=invalid-name, E1101
|
# pylint: disable=invalid-name, E1101
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
import numpy as np
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
from mpl_toolkits.mplot3d import Axes3D
|
|
||||||
|
|
||||||
import gtsam
|
import gtsam
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import numpy as np
|
||||||
from gtsam.utils import plot
|
from gtsam.utils import plot
|
||||||
|
from mpl_toolkits.mplot3d import Axes3D
|
||||||
|
|
||||||
|
|
||||||
def vector6(x, y, z, a, b, c):
|
def vector6(x, y, z, a, b, c):
|
||||||
|
|
@ -22,50 +23,62 @@ def vector6(x, y, z, a, b, c):
|
||||||
return np.array([x, y, z, a, b, c], dtype=float)
|
return np.array([x, y, z, a, b, c], dtype=float)
|
||||||
|
|
||||||
|
|
||||||
parser = argparse.ArgumentParser(
|
def main():
|
||||||
description="A 3D Pose SLAM example that reads input from g2o, and "
|
"""Main runner."""
|
||||||
"initializes Pose3")
|
|
||||||
parser.add_argument('-i', '--input', help='input file g2o format')
|
|
||||||
parser.add_argument('-o', '--output',
|
|
||||||
help="the path to the output file with optimized graph")
|
|
||||||
parser.add_argument("-p", "--plot", action="store_true",
|
|
||||||
help="Flag to plot results")
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
|
parser = argparse.ArgumentParser(
|
||||||
else args.input
|
description="A 3D Pose SLAM example that reads input from g2o, and "
|
||||||
|
"initializes Pose3")
|
||||||
|
parser.add_argument('-i', '--input', help='input file g2o format')
|
||||||
|
parser.add_argument(
|
||||||
|
'-o',
|
||||||
|
'--output',
|
||||||
|
help="the path to the output file with optimized graph")
|
||||||
|
parser.add_argument("-p",
|
||||||
|
"--plot",
|
||||||
|
action="store_true",
|
||||||
|
help="Flag to plot results")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
is3D = True
|
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
|
||||||
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
else args.input
|
||||||
|
|
||||||
# Add Prior on the first key
|
is3D = True
|
||||||
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
|
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
||||||
1e-4, 1e-4, 1e-4))
|
|
||||||
|
|
||||||
print("Adding prior to g2o file ")
|
# Add Prior on the first key
|
||||||
firstKey = initial.keys()[0]
|
priorModel = gtsam.noiseModel.Diagonal.Variances(
|
||||||
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
|
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
|
||||||
|
|
||||||
params = gtsam.GaussNewtonParams()
|
print("Adding prior to g2o file ")
|
||||||
params.setVerbosity("Termination") # this will show info about stopping conds
|
firstKey = initial.keys()[0]
|
||||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
|
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
|
||||||
result = optimizer.optimize()
|
|
||||||
print("Optimization complete")
|
|
||||||
|
|
||||||
print("initial error = ", graph.error(initial))
|
params = gtsam.GaussNewtonParams()
|
||||||
print("final error = ", graph.error(result))
|
params.setVerbosity(
|
||||||
|
"Termination") # this will show info about stopping conds
|
||||||
|
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
|
||||||
|
result = optimizer.optimize()
|
||||||
|
print("Optimization complete")
|
||||||
|
|
||||||
if args.output is None:
|
print("initial error = ", graph.error(initial))
|
||||||
print("Final Result:\n{}".format(result))
|
print("final error = ", graph.error(result))
|
||||||
else:
|
|
||||||
outputFile = args.output
|
|
||||||
print("Writing results to file: ", outputFile)
|
|
||||||
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
|
|
||||||
gtsam.writeG2o(graphNoKernel, result, outputFile)
|
|
||||||
print ("Done!")
|
|
||||||
|
|
||||||
if args.plot:
|
if args.output is None:
|
||||||
resultPoses = gtsam.utilities.allPose3s(result)
|
print("Final Result:\n{}".format(result))
|
||||||
for i in range(resultPoses.size()):
|
else:
|
||||||
plot.plot_pose3(1, resultPoses.atPose3(i))
|
outputFile = args.output
|
||||||
plt.show()
|
print("Writing results to file: ", outputFile)
|
||||||
|
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
|
||||||
|
gtsam.writeG2o(graphNoKernel, result, outputFile)
|
||||||
|
print("Done!")
|
||||||
|
|
||||||
|
if args.plot:
|
||||||
|
resultPoses = gtsam.utilities.allPose3s(result)
|
||||||
|
for i in range(resultPoses.size()):
|
||||||
|
plot.plot_pose3(1, resultPoses.atPose3(i))
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
|
|
@ -13,23 +13,29 @@ Author: Luca Carlone, Frank Dellaert (python port)
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
|
||||||
|
import gtsam
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
import gtsam
|
|
||||||
|
|
||||||
# Read graph from file
|
def main():
|
||||||
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
|
"""Main runner."""
|
||||||
|
# Read graph from file
|
||||||
|
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
|
||||||
|
|
||||||
is3D = True
|
is3D = True
|
||||||
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
||||||
|
|
||||||
# Add prior on the first key. TODO: assumes first key ios z
|
# Add prior on the first key. TODO: assumes first key ios z
|
||||||
priorModel = gtsam.noiseModel.Diagonal.Variances(
|
priorModel = gtsam.noiseModel.Diagonal.Variances(
|
||||||
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
|
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
|
||||||
firstKey = initial.keys()[0]
|
firstKey = initial.keys()[0]
|
||||||
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
|
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
|
||||||
|
|
||||||
# Initializing Pose3 - chordal relaxation"
|
# Initializing Pose3 - chordal relaxation
|
||||||
initialization = gtsam.InitializePose3.initialize(graph)
|
initialization = gtsam.InitializePose3.initialize(graph)
|
||||||
|
|
||||||
print(initialization)
|
print(initialization)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue