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