Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixup Python Examples #903

Merged
merged 2 commits into from
Oct 23, 2021
Merged

Fixup Python Examples #903

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
430530c
put all example scripts inside main() functions and apply formatting
varunagrawal Oct 22, 2021
f8f93ca
add type annotations
varunagrawal Oct 22, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
229 changes: 128 additions & 101 deletions python/gtsam/examples/CustomFactorExample.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,17 @@
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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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."""

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
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()

params = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
error = np.array([(result.atVector(unknown[k]) - x[k])[0]
for k in range(5)])

result = optimizer.optimize()
print("Result with GPS+Odometry+Landmark")
print(result, np.round(error, 2),
f"\nJ(X)={0.5 * np.sum(np.square(error))}")

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()
51 changes: 26 additions & 25 deletions python/gtsam/examples/GPSFactorExample.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,8 @@

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
Expand All @@ -29,28 +24,34 @@
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 a prior on the first point, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose3() # prior at origin
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))

# Add GPS factors
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
print("\nFactor Graph:\n{}".format(graph))

# Add GPS factors
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose3())
print("\nInitial Estimate:\n{}".format(initial))

# 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))

# 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()
Loading