Add unstable files
Fan Jiang
parent
c0c2564ac6
commit
7873c36088
|
|
@ -0,0 +1 @@
|
|||
from .gtsam_unstable import *
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Demonstration of the fixed-lag smoothers using a planar robot example
|
||||
and multiple odometry-like sensors
|
||||
Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
import gtsam_unstable
|
||||
|
||||
|
||||
def _timestamp_key_value(key, value):
|
||||
"""
|
||||
|
||||
"""
|
||||
return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
|
||||
key, value
|
||||
)
|
||||
|
||||
|
||||
def BatchFixedLagSmootherExample():
|
||||
"""
|
||||
Runs a batch fixed smoother on an agent with two odometry
|
||||
sensors that is simply moving to the
|
||||
"""
|
||||
|
||||
# Define a batch fixed lag smoother, which uses
|
||||
# Levenberg-Marquardt to perform the nonlinear optimization
|
||||
lag = 2.0
|
||||
smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
|
||||
|
||||
# Create containers to store the factors and linearization points
|
||||
# that will be sent to the smoothers
|
||||
new_factors = gtsam.NonlinearFactorGraph()
|
||||
new_values = gtsam.Values()
|
||||
new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
|
||||
|
||||
# Create a prior on the first pose, placing it at the origin
|
||||
prior_mean = gtsam.Pose2(0, 0, 0)
|
||||
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
X1 = 0
|
||||
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
|
||||
new_values.insert(X1, prior_mean)
|
||||
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
|
||||
|
||||
delta_time = 0.25
|
||||
time = 0.25
|
||||
|
||||
while time <= 3.0:
|
||||
previous_key = 1000 * (time - delta_time)
|
||||
current_key = 1000 * time
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps.insert(_timestamp_key_value(current_key, time))
|
||||
|
||||
# Add a guess for this pose to the new values
|
||||
# Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
|
||||
current_pose = gtsam.Pose2(time * 2, 0, 0)
|
||||
new_values.insert(current_key, current_pose)
|
||||
|
||||
# Add odometry factors from two different sources with different error
|
||||
# stats
|
||||
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
|
||||
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.1, 0.1, 0.05]))
|
||||
new_factors.push_back(gtsam.BetweenFactorPose2(
|
||||
previous_key, current_key, odometry_measurement_1, odometry_noise_1
|
||||
))
|
||||
|
||||
odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
|
||||
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05]))
|
||||
new_factors.push_back(gtsam.BetweenFactorPose2(
|
||||
previous_key, current_key, odometry_measurement_2, odometry_noise_2
|
||||
))
|
||||
|
||||
# Update the smoothers with the new factors. In this case,
|
||||
# one iteration must pass for Levenberg-Marquardt to accurately
|
||||
# estimate
|
||||
if time >= 0.50:
|
||||
smoother_batch.update(new_factors, new_values, new_timestamps)
|
||||
print("Timestamp = " + str(time) + ", Key = " + str(current_key))
|
||||
print(smoother_batch.calculateEstimatePose2(current_key))
|
||||
|
||||
new_timestamps.clear()
|
||||
new_values.clear()
|
||||
new_factors.resize(0)
|
||||
|
||||
time += delta_time
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
BatchFixedLagSmootherExample()
|
||||
print("Example complete")
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Track a moving object "Time of Arrival" measurements at 4 microphones.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, no-name-in-module
|
||||
|
||||
from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
|
||||
NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
|
||||
from gtsam_unstable import Event, TimeOfArrival, TOAFactor
|
||||
|
||||
# units
|
||||
MS = 1e-3
|
||||
CM = 1e-2
|
||||
|
||||
# Instantiate functor with speed of sound value
|
||||
TIME_OF_ARRIVAL = TimeOfArrival(330)
|
||||
|
||||
|
||||
def define_microphones():
|
||||
"""Create microphones."""
|
||||
height = 0.5
|
||||
microphones = []
|
||||
microphones.append(Point3(0, 0, height))
|
||||
microphones.append(Point3(403 * CM, 0, height))
|
||||
microphones.append(Point3(403 * CM, 403 * CM, height))
|
||||
microphones.append(Point3(0, 403 * CM, 2 * height))
|
||||
return microphones
|
||||
|
||||
|
||||
def create_trajectory(n):
|
||||
"""Create ground truth trajectory."""
|
||||
trajectory = []
|
||||
timeOfEvent = 10
|
||||
# simulate emitting a sound every second while moving on straight line
|
||||
for key in range(n):
|
||||
trajectory.append(
|
||||
Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
|
||||
timeOfEvent += 1
|
||||
|
||||
return trajectory
|
||||
|
||||
|
||||
def simulate_one_toa(microphones, event):
|
||||
"""Simulate time-of-arrival measurements for a single event."""
|
||||
return [TIME_OF_ARRIVAL.measure(event, microphones[i])
|
||||
for i in range(len(microphones))]
|
||||
|
||||
|
||||
def simulate_toa(microphones, trajectory):
|
||||
"""Simulate time-of-arrival measurements for an entire trajectory."""
|
||||
return [simulate_one_toa(microphones, event)
|
||||
for event in trajectory]
|
||||
|
||||
|
||||
def create_graph(microphones, simulatedTOA):
|
||||
"""Create factor graph."""
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Create a noise model for the TOA error
|
||||
model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
|
||||
|
||||
K = len(microphones)
|
||||
key = 0
|
||||
for toa in simulatedTOA:
|
||||
for i in range(K):
|
||||
factor = TOAFactor(key, microphones[i], toa[i], model)
|
||||
graph.push_back(factor)
|
||||
key += 1
|
||||
|
||||
return graph
|
||||
|
||||
|
||||
def create_initial_estimate(n):
|
||||
"""Create initial estimate for n events."""
|
||||
initial = Values()
|
||||
zero = Event()
|
||||
for key in range(n):
|
||||
TOAFactor.InsertEvent(key, zero, initial)
|
||||
return initial
|
||||
|
||||
|
||||
def toa_example():
|
||||
"""Run example with 4 microphones and 5 events in a straight line."""
|
||||
# Create microphones
|
||||
microphones = define_microphones()
|
||||
K = len(microphones)
|
||||
for i in range(K):
|
||||
print("mic {} = {}".format(i, microphones[i]))
|
||||
|
||||
# Create a ground truth trajectory
|
||||
n = 5
|
||||
groundTruth = create_trajectory(n)
|
||||
for event in groundTruth:
|
||||
print(event)
|
||||
|
||||
# Simulate time-of-arrival measurements
|
||||
simulatedTOA = simulate_toa(microphones, groundTruth)
|
||||
for key in range(n):
|
||||
for i in range(K):
|
||||
print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
|
||||
|
||||
# create factor graph
|
||||
graph = create_graph(microphones, simulatedTOA)
|
||||
print(graph.at(0))
|
||||
|
||||
# Create initial estimate
|
||||
initial_estimate = create_initial_estimate(n)
|
||||
print(initial_estimate)
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization.
|
||||
params = LevenbergMarquardtParams()
|
||||
params.setAbsoluteErrorTol(1e-10)
|
||||
params.setVerbosityLM("SUMMARY")
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
|
||||
result = optimizer.optimize()
|
||||
print("Final Result:\n", result)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
toa_example()
|
||||
print("Example complete")
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Cal3Unified unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
import gtsam_unstable
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
def _timestamp_key_value(key, value):
|
||||
return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
|
||||
key, value
|
||||
)
|
||||
|
||||
|
||||
class TestFixedLagSmootherExample(GtsamTestCase):
|
||||
'''
|
||||
Tests the fixed lag smoother wrapper
|
||||
'''
|
||||
|
||||
def test_FixedLagSmootherExample(self):
|
||||
'''
|
||||
Simple test that checks for equality between C++ example
|
||||
file and the Python implementation. See
|
||||
gtsam_unstable/examples/FixedLagSmootherExample.cpp
|
||||
'''
|
||||
# Define a batch fixed lag smoother, which uses
|
||||
# Levenberg-Marquardt to perform the nonlinear optimization
|
||||
lag = 2.0
|
||||
smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
|
||||
|
||||
# Create containers to store the factors and linearization points
|
||||
# that will be sent to the smoothers
|
||||
new_factors = gtsam.NonlinearFactorGraph()
|
||||
new_values = gtsam.Values()
|
||||
new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
|
||||
|
||||
# Create a prior on the first pose, placing it at the origin
|
||||
prior_mean = gtsam.Pose2(0, 0, 0)
|
||||
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.1]))
|
||||
X1 = 0
|
||||
new_factors.push_back(
|
||||
gtsam.PriorFactorPose2(
|
||||
X1, prior_mean, prior_noise))
|
||||
new_values.insert(X1, prior_mean)
|
||||
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
|
||||
|
||||
delta_time = 0.25
|
||||
time = 0.25
|
||||
|
||||
i = 0
|
||||
|
||||
ground_truth = [
|
||||
gtsam.Pose2(0.995821, 0.0231012, 0.0300001),
|
||||
gtsam.Pose2(1.49284, 0.0457247, 0.045),
|
||||
gtsam.Pose2(1.98981, 0.0758879, 0.06),
|
||||
gtsam.Pose2(2.48627, 0.113502, 0.075),
|
||||
gtsam.Pose2(2.98211, 0.158558, 0.09),
|
||||
gtsam.Pose2(3.47722, 0.211047, 0.105),
|
||||
gtsam.Pose2(3.97149, 0.270956, 0.12),
|
||||
gtsam.Pose2(4.4648, 0.338272, 0.135),
|
||||
gtsam.Pose2(4.95705, 0.41298, 0.15),
|
||||
gtsam.Pose2(5.44812, 0.495063, 0.165),
|
||||
gtsam.Pose2(5.9379, 0.584503, 0.18),
|
||||
]
|
||||
|
||||
# Iterates from 0.25s to 3.0s, adding 0.25s each loop
|
||||
# In each iteration, the agent moves at a constant speed
|
||||
# and its two odometers measure the change. The smoothed
|
||||
# result is then compared to the ground truth
|
||||
while time <= 3.0:
|
||||
previous_key = 1000 * (time - delta_time)
|
||||
current_key = 1000 * time
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps.insert(_timestamp_key_value(current_key, time))
|
||||
|
||||
# Add a guess for this pose to the new values
|
||||
# Assume that the robot moves at 2 m/s. Position is time[s] *
|
||||
# 2[m/s]
|
||||
current_pose = gtsam.Pose2(time * 2, 0, 0)
|
||||
new_values.insert(current_key, current_pose)
|
||||
|
||||
# Add odometry factors from two different sources with different
|
||||
# error stats
|
||||
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
|
||||
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.1, 0.1, 0.05]))
|
||||
new_factors.push_back(
|
||||
gtsam.BetweenFactorPose2(
|
||||
previous_key,
|
||||
current_key,
|
||||
odometry_measurement_1,
|
||||
odometry_noise_1))
|
||||
|
||||
odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
|
||||
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05]))
|
||||
new_factors.push_back(
|
||||
gtsam.BetweenFactorPose2(
|
||||
previous_key,
|
||||
current_key,
|
||||
odometry_measurement_2,
|
||||
odometry_noise_2))
|
||||
|
||||
# Update the smoothers with the new factors. In this case,
|
||||
# one iteration must pass for Levenberg-Marquardt to accurately
|
||||
# estimate
|
||||
if time >= 0.50:
|
||||
smoother_batch.update(new_factors, new_values, new_timestamps)
|
||||
|
||||
estimate = smoother_batch.calculateEstimatePose2(current_key)
|
||||
self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
|
||||
i += 1
|
||||
|
||||
new_timestamps.clear()
|
||||
new_values.clear()
|
||||
new_factors.resize(0)
|
||||
|
||||
time += delta_time
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue