Remove python folder temporarily
Fan Jiang
parent
e87fd24df1
commit
bb6aea3b8f
|
|
@ -1,104 +0,0 @@
|
|||
set(GTSAM_PYTHON_BUILD_DIRECTORY ${PROJECT_BINARY_DIR}/python)
|
||||
|
||||
if(GTSAM_BUILD_PYTHON)
|
||||
# Generate setup.py.
|
||||
file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
|
||||
configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in
|
||||
${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py)
|
||||
|
||||
set(WRAP_USE_CUSTOM_PYTHON_LIBRARY ${GTSAM_USE_CUSTOM_PYTHON_LIBRARY})
|
||||
set(WRAP_PYTHON_VERSION ${GTSAM_PYTHON_VERSION})
|
||||
|
||||
include(PybindWrap)
|
||||
|
||||
add_custom_target(gtsam_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam/gtsam.i")
|
||||
add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i")
|
||||
|
||||
# ignoring the non-concrete types (type aliases)
|
||||
set(ignore
|
||||
gtsam::Point2
|
||||
gtsam::Point3
|
||||
gtsam::LieVector
|
||||
gtsam::LieMatrix
|
||||
gtsam::ISAM2ThresholdMapValue
|
||||
gtsam::FactorIndices
|
||||
gtsam::FactorIndexSet
|
||||
gtsam::BetweenFactorPose3s
|
||||
gtsam::Point2Vector
|
||||
gtsam::Pose3Vector
|
||||
gtsam::KeyVector)
|
||||
|
||||
pybind_wrap(gtsam_py # target
|
||||
${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
|
||||
"gtsam.cpp" # generated_cpp
|
||||
"gtsam" # module_name
|
||||
"gtsam" # top_namespace
|
||||
"${ignore}" # ignore_classes
|
||||
${PROJECT_SOURCE_DIR}/python/gtsam/gtsam.tpl
|
||||
gtsam # libs
|
||||
"gtsam;gtsam_header" # dependencies
|
||||
ON # use_boost
|
||||
)
|
||||
|
||||
set_target_properties(gtsam_py PROPERTIES
|
||||
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
|
||||
INSTALL_RPATH_USE_LINK_PATH TRUE
|
||||
OUTPUT_NAME "gtsam"
|
||||
LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam"
|
||||
DEBUG_POSTFIX "" # Otherwise you will have a wrong name
|
||||
RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name
|
||||
)
|
||||
|
||||
set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
|
||||
|
||||
# Symlink all tests .py files to build folder.
|
||||
create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
|
||||
"${GTSAM_MODULE_PATH}")
|
||||
|
||||
if(GTSAM_UNSTABLE_BUILD_PYTHON)
|
||||
set(ignore
|
||||
gtsam::Point2
|
||||
gtsam::Point3
|
||||
gtsam::LieVector
|
||||
gtsam::LieMatrix
|
||||
gtsam::ISAM2ThresholdMapValue
|
||||
gtsam::FactorIndices
|
||||
gtsam::FactorIndexSet
|
||||
gtsam::BetweenFactorPose3s
|
||||
gtsam::Point2Vector
|
||||
gtsam::Pose3Vector
|
||||
gtsam::KeyVector
|
||||
gtsam::FixedLagSmootherKeyTimestampMapValue)
|
||||
pybind_wrap(gtsam_unstable_py # target
|
||||
${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
|
||||
"gtsam_unstable.cpp" # generated_cpp
|
||||
"gtsam_unstable" # module_name
|
||||
"gtsam" # top_namespace
|
||||
"${ignore}" # ignore_classes
|
||||
${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl
|
||||
gtsam_unstable # libs
|
||||
"gtsam_unstable;gtsam_unstable_header" # dependencies
|
||||
ON # use_boost
|
||||
)
|
||||
|
||||
set_target_properties(gtsam_unstable_py PROPERTIES
|
||||
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
|
||||
INSTALL_RPATH_USE_LINK_PATH TRUE
|
||||
OUTPUT_NAME "gtsam_unstable"
|
||||
LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable"
|
||||
DEBUG_POSTFIX "" # Otherwise you will have a wrong name
|
||||
RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name
|
||||
)
|
||||
|
||||
set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable)
|
||||
|
||||
# Symlink all tests .py files to build folder.
|
||||
create_symlinks("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable"
|
||||
"${GTSAM_UNSTABLE_MODULE_PATH}")
|
||||
endif()
|
||||
|
||||
set(GTSAM_PYTHON_INSTALL_TARGET python-install)
|
||||
add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install
|
||||
WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
|
||||
endif()
|
||||
|
|
@ -1,87 +0,0 @@
|
|||
# README
|
||||
|
||||
# Python Wrapper
|
||||
|
||||
This is the Python wrapper around the GTSAM C++ library. We use Cython to generate the bindings to the underlying C++ code.
|
||||
|
||||
## Requirements
|
||||
|
||||
- If you want to build the GTSAM python library for a specific python version (eg 3.6),
|
||||
use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
|
||||
- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),
|
||||
then the environment should be active while building GTSAM.
|
||||
- This wrapper needs `Cython(>=0.25.2)`, `backports_abc(>=0.5)`, and `numpy(>=1.11.0)`. These can be installed as follows:
|
||||
|
||||
```bash
|
||||
pip install -r <gtsam_folder>/cython/requirements.txt
|
||||
```
|
||||
|
||||
- For compatibility with GTSAM's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
|
||||
named `gtsam_eigency`, to interface between C++'s Eigen and Python's numpy.
|
||||
|
||||
## Install
|
||||
|
||||
- Run cmake with the `GTSAM_INSTALL_CYTHON_TOOLBOX` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory defined by `GTSAM_CYTHON_INSTALL_PATH`, which is by default `<PROJECT_BINARY_DIR>/cython` in Release mode and `<PROJECT_BINARY_DIR>/cython<CMAKE_BUILD_TYPE>` for other modes.
|
||||
|
||||
- Build GTSAM and the wrapper with `make`.
|
||||
|
||||
- To install, simply run `make python-install`.
|
||||
- The same command can be used to install into a virtual environment if it is active.
|
||||
- **NOTE**: if you don't want GTSAM to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install GTSAM to a subdirectory of the build directory.
|
||||
|
||||
- You can also directly run `make python-install` without running `make`, and it will compile all the dependencies accordingly.
|
||||
|
||||
## Unit Tests
|
||||
|
||||
The Cython toolbox also has a small set of unit tests located in the
|
||||
test directory. To run them:
|
||||
|
||||
```bash
|
||||
cd <GTSAM_CYTHON_INSTALL_PATH>
|
||||
python -m unittest discover
|
||||
```
|
||||
|
||||
## Utils
|
||||
|
||||
TODO
|
||||
|
||||
## Examples
|
||||
|
||||
TODO
|
||||
|
||||
## Writing Your Own Scripts
|
||||
|
||||
See the tests for examples.
|
||||
|
||||
### Some Important Notes:
|
||||
|
||||
- Vector/Matrix:
|
||||
|
||||
- GTSAM expects double-precision floating point vectors and matrices.
|
||||
Hence, you should pass numpy matrices with `dtype=float`, or `float64`.
|
||||
- Also, GTSAM expects _column-major_ matrices, unlike the default storage
|
||||
scheme in numpy. Hence, you should pass column-major matrices to GTSAM using
|
||||
the flag order='F'. And you always get column-major matrices back.
|
||||
For more details, see [this link](https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed).
|
||||
- Passing row-major matrices of different dtype, e.g. `int`, will also work
|
||||
as the wrapper converts them to column-major and dtype float for you,
|
||||
using numpy.array.astype(float, order='F', copy=False).
|
||||
However, this will result a copy if your matrix is not in the expected type
|
||||
and storage order.
|
||||
|
||||
- Inner namespace: Classes in inner namespace will be prefixed by <innerNamespace>\_ in Python.
|
||||
|
||||
Examples: `noiseModel_Gaussian`, `noiseModel_mEstimator_Tukey`
|
||||
|
||||
- Casting from a base class to a derive class must be done explicitly.
|
||||
|
||||
Examples:
|
||||
|
||||
```python
|
||||
noiseBase = factor.noiseModel()
|
||||
noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
|
||||
```
|
||||
|
||||
## Wrapping Custom GTSAM-based Project
|
||||
|
||||
Please refer to the template project and the corresponding tutorial available [here](https://github.com/borglab/GTSAM-project-python).
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
from .gtsam import *
|
||||
|
||||
|
||||
def _init():
|
||||
"""This function is to add shims for the long-gone Point2 and Point3 types"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
global Point2 # export function
|
||||
|
||||
def Point2(x=0, y=0):
|
||||
"""Shim for the deleted Point2 type."""
|
||||
return np.array([x, y], dtype=float)
|
||||
|
||||
global Point3 # export function
|
||||
|
||||
def Point3(x=0, y=0, z=0):
|
||||
"""Shim for the deleted Point3 type."""
|
||||
return np.array([x, y, z], dtype=float)
|
||||
|
||||
# for interactive debugging
|
||||
if __name__ == "__main__":
|
||||
# we want all definitions accessible
|
||||
globals().update(locals())
|
||||
|
||||
|
||||
_init()
|
||||
|
|
@ -1,118 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Example comparing DoglegOptimizer with Levenberg-Marquardt.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-member, invalid-name
|
||||
|
||||
import math
|
||||
import argparse
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
|
||||
def run(args):
|
||||
"""Test Dogleg vs LM, inspired by issue #452."""
|
||||
|
||||
# print parameters
|
||||
print("num samples = {}, deltaInitial = {}".format(
|
||||
args.num_samples, args.delta))
|
||||
|
||||
# Ground truth solution
|
||||
T11 = gtsam.Pose2(0, 0, 0)
|
||||
T12 = gtsam.Pose2(1, 0, 0)
|
||||
T21 = gtsam.Pose2(0, 1, 0)
|
||||
T22 = gtsam.Pose2(1, 1, 0)
|
||||
|
||||
# Factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Priors
|
||||
prior = gtsam.noiseModel.Isotropic.Sigma(3, 1)
|
||||
graph.add(gtsam.PriorFactorPose2(11, T11, prior))
|
||||
graph.add(gtsam.PriorFactorPose2(21, T21, prior))
|
||||
|
||||
# Odometry
|
||||
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
|
||||
graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
|
||||
graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
|
||||
|
||||
# Range
|
||||
model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)
|
||||
graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
|
||||
|
||||
params = gtsam.DoglegParams()
|
||||
params.setDeltaInitial(args.delta) # default is 10
|
||||
|
||||
# Add progressively more noise to ground truth
|
||||
sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
|
||||
n = len(sigmas)
|
||||
p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n
|
||||
for i, sigma in enumerate(sigmas):
|
||||
dl_fails, lm_fails = 0, 0
|
||||
# Attempt num_samples optimizations for both DL and LM
|
||||
for _attempt in range(args.num_samples):
|
||||
initial = gtsam.Values()
|
||||
initial.insert(11, T11.retract(np.random.normal(0, sigma, 3)))
|
||||
initial.insert(12, T12.retract(np.random.normal(0, sigma, 3)))
|
||||
initial.insert(21, T21.retract(np.random.normal(0, sigma, 3)))
|
||||
initial.insert(22, T22.retract(np.random.normal(0, sigma, 3)))
|
||||
|
||||
# Run dogleg optimizer
|
||||
dl = gtsam.DoglegOptimizer(graph, initial, params)
|
||||
result = dl.optimize()
|
||||
dl_fails += graph.error(result) > 1e-9
|
||||
|
||||
# Run
|
||||
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial)
|
||||
result = lm.optimize()
|
||||
lm_fails += graph.error(result) > 1e-9
|
||||
|
||||
# Calculate Bayes estimate of success probability
|
||||
# using a beta prior of alpha=0.5, beta=0.5
|
||||
alpha, beta = 0.5, 0.5
|
||||
v = args.num_samples+alpha+beta
|
||||
p_dl[i] = (args.num_samples-dl_fails+alpha)/v
|
||||
p_lm[i] = (args.num_samples-lm_fails+alpha)/v
|
||||
|
||||
def stddev(p):
|
||||
"""Calculate standard deviation."""
|
||||
return math.sqrt(p*(1-p)/(1+v))
|
||||
|
||||
s_dl[i] = stddev(p_dl[i])
|
||||
s_lm[i] = stddev(p_lm[i])
|
||||
|
||||
fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%"
|
||||
print(fmt.format(sigma,
|
||||
100*p_dl[i], 100*s_dl[i],
|
||||
100*p_lm[i], 100*s_lm[i]))
|
||||
|
||||
if args.plot:
|
||||
fig, ax = plt.subplots()
|
||||
dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg")
|
||||
lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM")
|
||||
plt.title("Dogleg emprical success vs. LM")
|
||||
plt.legend(handles=[dl_plot, lm_plot])
|
||||
ax.set_xlim(0, sigmas[-1]+1)
|
||||
ax.set_ylim(0, 1)
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Compare Dogleg and LM success rates")
|
||||
parser.add_argument("-n", "--num_samples", type=int, default=1000,
|
||||
help="Number of samples for each sigma")
|
||||
parser.add_argument("-d", "--delta", type=float, default=10.0,
|
||||
help="Initial delta for dogleg")
|
||||
parser.add_argument("-p", "--plot", action="store_true",
|
||||
help="Flag to plot results")
|
||||
args = parser.parse_args()
|
||||
run(args)
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
Simple robot motion example, with prior and one GPS measurements
|
||||
Author: Mandy Xie
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
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
|
||||
h0 = 274
|
||||
|
||||
# Create noise models
|
||||
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))
|
||||
|
||||
# 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))
|
||||
|
||||
# optimize using Levenberg-Marquardt optimization
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
print("\nFinal Result:\n{}".format(result))
|
||||
|
||||
|
|
@ -1,180 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A script validating and demonstrating the ImuFactor inference.
|
||||
|
||||
Author: Frank Dellaert, Varun Agrawal
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import math
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import B, V, X
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
from PreintegrationExample import POSES_FIG, PreintegrationExample
|
||||
|
||||
BIAS_KEY = B(0)
|
||||
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
|
||||
|
||||
class ImuFactorExample(PreintegrationExample):
|
||||
|
||||
def __init__(self, twist_scenario="sick_twist"):
|
||||
self.velocity = np.array([2, 0, 0])
|
||||
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
|
||||
# Choose one of these twists to change scenario:
|
||||
twist_scenarios = dict(
|
||||
zero_twist=(np.zeros(3), np.zeros(3)),
|
||||
forward_twist=(np.zeros(3), self.velocity),
|
||||
loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
|
||||
sick_twist=(np.array([math.radians(30), -math.radians(30), 0]),
|
||||
self.velocity)
|
||||
)
|
||||
|
||||
accBias = np.array([-0.3, 0.1, 0.2])
|
||||
gyroBias = np.array([0.1, 0.3, -0.1])
|
||||
bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
|
||||
|
||||
dt = 1e-2
|
||||
super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
|
||||
bias, dt)
|
||||
|
||||
def addPrior(self, i, graph):
|
||||
state = self.scenario.navState(i)
|
||||
graph.push_back(gtsam.PriorFactorPose3(
|
||||
X(i), state.pose(), self.priorNoise))
|
||||
graph.push_back(gtsam.PriorFactorVector(
|
||||
V(i), state.velocity(), self.velNoise))
|
||||
|
||||
def run(self, T=12, compute_covariances=False, verbose=True):
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# initialize data structure for pre-integrated IMU measurements
|
||||
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
|
||||
|
||||
T = 12
|
||||
num_poses = T # assumes 1 factor per second
|
||||
initial = gtsam.Values()
|
||||
initial.insert(BIAS_KEY, self.actualBias)
|
||||
|
||||
# simulate the loop
|
||||
i = 0 # state index
|
||||
initial_state_i = self.scenario.navState(0)
|
||||
initial.insert(X(i), initial_state_i.pose())
|
||||
initial.insert(V(i), initial_state_i.velocity())
|
||||
|
||||
# add prior on beginning
|
||||
self.addPrior(0, graph)
|
||||
|
||||
for k, t in enumerate(np.arange(0, T, self.dt)):
|
||||
# get measurements and add them to PIM
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
|
||||
|
||||
# Plot IMU many times
|
||||
if k % 10 == 0:
|
||||
self.plotImu(t, measuredOmega, measuredAcc)
|
||||
|
||||
if (k+1) % int(1 / self.dt) == 0:
|
||||
# Plot every second
|
||||
self.plotGroundTruthPose(t, scale=1)
|
||||
plt.title("Ground Truth Trajectory")
|
||||
|
||||
# create IMU factor every second
|
||||
factor = gtsam.ImuFactor(X(i), V(i),
|
||||
X(i + 1), V(i + 1),
|
||||
BIAS_KEY, pim)
|
||||
graph.push_back(factor)
|
||||
|
||||
if verbose:
|
||||
print(factor)
|
||||
print(pim.predict(actual_state_i, self.actualBias))
|
||||
|
||||
pim.resetIntegration()
|
||||
|
||||
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
|
||||
translationNoise = gtsam.Point3(*np.random.randn(3)*1)
|
||||
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
|
||||
|
||||
actual_state_i = self.scenario.navState(t + self.dt)
|
||||
print("Actual state at {0}:\n{1}".format(
|
||||
t+self.dt, actual_state_i))
|
||||
|
||||
noisy_state_i = gtsam.NavState(
|
||||
actual_state_i.pose().compose(poseNoise),
|
||||
actual_state_i.velocity() + np.random.randn(3)*0.1)
|
||||
|
||||
initial.insert(X(i+1), noisy_state_i.pose())
|
||||
initial.insert(V(i+1), noisy_state_i.velocity())
|
||||
i += 1
|
||||
|
||||
# add priors on end
|
||||
# self.addPrior(num_poses - 1, graph)
|
||||
|
||||
initial.print_("Initial values:")
|
||||
|
||||
# optimize using Levenberg-Marquardt optimization
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
params.setVerbosityLM("SUMMARY")
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
|
||||
result.print_("Optimized values:")
|
||||
|
||||
if compute_covariances:
|
||||
# Calculate and print marginal covariances
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
print("Covariance on bias:\n",
|
||||
marginals.marginalCovariance(BIAS_KEY))
|
||||
for i in range(num_poses):
|
||||
print("Covariance on pose {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(X(i))))
|
||||
print("Covariance on vel {}:\n{}\n".format(
|
||||
i, marginals.marginalCovariance(V(i))))
|
||||
|
||||
# Plot resulting poses
|
||||
i = 0
|
||||
while result.exists(X(i)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
plot_pose3(POSES_FIG+1, pose_i, 1)
|
||||
i += 1
|
||||
plt.title("Estimated Trajectory")
|
||||
|
||||
gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
|
||||
|
||||
print("Bias Values", result.atConstantBias(BIAS_KEY))
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser("ImuFactorExample.py")
|
||||
parser.add_argument("--twist_scenario",
|
||||
default="sick_twist",
|
||||
choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist"))
|
||||
parser.add_argument("--time", "-T", default=12,
|
||||
type=int, help="Total time in seconds")
|
||||
parser.add_argument("--compute_covariances",
|
||||
default=False, action='store_true')
|
||||
parser.add_argument("--verbose", default=False, action='store_true')
|
||||
args = parser.parse_args()
|
||||
|
||||
ImuFactorExample(args.twist_scenario).run(
|
||||
args.time, args.compute_covariances, args.verbose)
|
||||
|
|
@ -1,166 +0,0 @@
|
|||
"""
|
||||
iSAM2 example with ImuFactor.
|
||||
Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
|
||||
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
|
||||
PinholeCameraCal3_S2, Point3, Pose3,
|
||||
PriorFactorConstantBias, PriorFactorPose3,
|
||||
PriorFactorVector, Rot3, Values)
|
||||
from gtsam.symbol_shorthand import B, V, X
|
||||
from gtsam.utils import plot
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
g = 9.81
|
||||
n_gravity = vector3(0, 0, -g)
|
||||
|
||||
|
||||
def preintegration_parameters():
|
||||
# IMU preintegration parameters
|
||||
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
|
||||
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||
I = np.eye(3)
|
||||
PARAMS.setAccelerometerCovariance(I * 0.1)
|
||||
PARAMS.setGyroscopeCovariance(I * 0.1)
|
||||
PARAMS.setIntegrationCovariance(I * 0.1)
|
||||
PARAMS.setUse2ndOrderCoriolis(False)
|
||||
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
|
||||
|
||||
BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
|
||||
DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
|
||||
Point3(0.05, -0.10, 0.20))
|
||||
|
||||
return PARAMS, BIAS_COVARIANCE, DELTA
|
||||
|
||||
|
||||
def get_camera(radius):
|
||||
up = Point3(0, 0, 1)
|
||||
target = Point3(0, 0, 0)
|
||||
position = Point3(radius, 0, 0)
|
||||
camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
|
||||
return camera
|
||||
|
||||
|
||||
def get_scenario(radius, pose_0, angular_velocity, delta_t):
|
||||
"""Create the set of ground-truth landmarks and poses"""
|
||||
|
||||
angular_velocity_vector = vector3(0, -angular_velocity, 0)
|
||||
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
|
||||
scenario = ConstantTwistScenario(
|
||||
angular_velocity_vector, linear_velocity_vector, pose_0)
|
||||
|
||||
return scenario
|
||||
|
||||
|
||||
def IMU_example():
|
||||
"""Run iSAM 2 example with IMU factor."""
|
||||
|
||||
# Start with a camera on x-axis looking at origin
|
||||
radius = 30
|
||||
camera = get_camera(radius)
|
||||
pose_0 = camera.pose()
|
||||
|
||||
delta_t = 1.0/18 # makes for 10 degrees per step
|
||||
angular_velocity = math.radians(180) # rad/sec
|
||||
scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
|
||||
|
||||
PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
|
||||
|
||||
# Create a factor graph
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Create (incremental) ISAM2 solver
|
||||
isam = ISAM2()
|
||||
|
||||
# Create the initial estimate to the solution
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initialEstimate = Values()
|
||||
|
||||
# Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noise = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
|
||||
graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
|
||||
|
||||
# Add imu priors
|
||||
biasKey = B(0)
|
||||
biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
|
||||
biasnoise)
|
||||
graph.push_back(biasprior)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
|
||||
velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
|
||||
# Calculate with correct initial velocity
|
||||
n_velocity = vector3(0, angular_velocity * radius, 0)
|
||||
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
|
||||
graph.push_back(velprior)
|
||||
initialEstimate.insert(V(0), n_velocity)
|
||||
|
||||
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
|
||||
|
||||
# Simulate poses and imu measurements, adding them to the factor graph
|
||||
for i in range(80):
|
||||
t = i * delta_t # simulation time
|
||||
if i == 0: # First time add two poses
|
||||
pose_1 = scenario.pose(delta_t)
|
||||
initialEstimate.insert(X(0), pose_0.compose(DELTA))
|
||||
initialEstimate.insert(X(1), pose_1.compose(DELTA))
|
||||
elif i >= 2: # Add more poses as necessary
|
||||
pose_i = scenario.pose(t)
|
||||
initialEstimate.insert(X(i), pose_i.compose(DELTA))
|
||||
|
||||
if i > 0:
|
||||
# Add Bias variables periodically
|
||||
if i % 5 == 0:
|
||||
biasKey += 1
|
||||
factor = BetweenFactorConstantBias(
|
||||
biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
|
||||
graph.add(factor)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
|
||||
|
||||
# Predict acceleration and gyro measurements in (actual) body frame
|
||||
nRb = scenario.rotation(t).matrix()
|
||||
bRn = np.transpose(nRb)
|
||||
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
|
||||
measuredOmega = scenario.omega_b(t)
|
||||
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
|
||||
|
||||
# Add Imu Factor
|
||||
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
|
||||
graph.add(imufac)
|
||||
|
||||
# insert new velocity, which is wrong
|
||||
initialEstimate.insert(V(i), n_velocity)
|
||||
accum.resetIntegration()
|
||||
|
||||
# Incremental solution
|
||||
isam.update(graph, initialEstimate)
|
||||
result = isam.calculateEstimate()
|
||||
plot.plot_incremental_trajectory(0, result,
|
||||
start=i, scale=3, time_interval=0.01)
|
||||
|
||||
# reset
|
||||
graph = NonlinearFactorGraph()
|
||||
initialEstimate.clear()
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
IMU_example()
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
Simple robot motion example, with prior and two odometry measurements
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
|
||||
# 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))
|
||||
|
||||
# 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))
|
||||
|
||||
# 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))
|
||||
|
||||
# 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)))
|
||||
|
||||
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()
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,332 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import unittest
|
||||
from functools import reduce
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
from gtsam import Pose2
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3D double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
def compose(*poses):
|
||||
"""Compose all Pose2 transforms given as arguments from left to right."""
|
||||
return reduce((lambda x, y: x.compose(y)), poses)
|
||||
|
||||
|
||||
def vee(M):
|
||||
"""Pose2 vee operator."""
|
||||
return vector3(M[0, 2], M[1, 2], M[1, 0])
|
||||
|
||||
|
||||
def delta(g0, g1):
|
||||
"""Difference between x,y,,theta components of SE(2) poses."""
|
||||
return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
|
||||
|
||||
|
||||
def trajectory(g0, g1, N=20):
|
||||
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
|
||||
g0 and g1 are the initial and final pose, respectively.
|
||||
N is the number of *intervals*
|
||||
Returns N+1 poses
|
||||
"""
|
||||
e = delta(g0, g1)
|
||||
return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
|
||||
|
||||
|
||||
class ThreeLinkArm(object):
|
||||
"""Three-link arm class."""
|
||||
|
||||
def __init__(self):
|
||||
self.L1 = 3.5
|
||||
self.L2 = 3.5
|
||||
self.L3 = 2.5
|
||||
self.xi1 = vector3(0, 0, 1)
|
||||
self.xi2 = vector3(self.L1, 0, 1)
|
||||
self.xi3 = vector3(self.L1+self.L2, 0, 1)
|
||||
self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
|
||||
|
||||
def fk(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
|
||||
|
||||
def jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
a = q[0]+q[1]
|
||||
b = a+q[2]
|
||||
return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
|
||||
-self.L1*math.cos(a)-self.L3*math.cos(b),
|
||||
- self.L3*math.cos(b)],
|
||||
[-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
|
||||
-self.L1*math.sin(a)-self.L3*math.sin(b),
|
||||
- self.L3*math.sin(b)],
|
||||
[1, 1, 1]], np.float)
|
||||
|
||||
def poe(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def con(self, q):
|
||||
""" Forward kinematics, conjugation form.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
def expmap(x, y, theta):
|
||||
"""Implement exponential map via conjugation with axis (x,y)."""
|
||||
return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
|
||||
|
||||
l1Zl1 = expmap(0.0, 0.0, q[0])
|
||||
l2Zl2 = expmap(0.0, self.L1, q[1])
|
||||
l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def ik(self, sTt_desired, e=1e-9):
|
||||
""" Inverse kinematics.
|
||||
Takes desired Pose2 of tool T with respect to base S.
|
||||
Optional: mu, gradient descent rate; e: error norm threshold
|
||||
"""
|
||||
q = np.radians(vector3(30, -30, 45)) # well within workspace
|
||||
error = vector3(100, 100, 100)
|
||||
|
||||
while np.linalg.norm(error) > e:
|
||||
error = delta(sTt_desired, self.fk(q))
|
||||
J = self.jacobian(q)
|
||||
q -= np.dot(np.linalg.pinv(J), error)
|
||||
|
||||
# return result in interval [-pi,pi)
|
||||
return np.remainder(q+math.pi, 2*math.pi)-math.pi
|
||||
|
||||
def manipulator_jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
Returns the manipulator Jacobian of differential twists. When multiplied with
|
||||
a vector of joint velocities, will yield a single differential twist which is
|
||||
the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
|
||||
Just like always, differential twists can be hatted and multiplied with spatial
|
||||
coordinates of a point to give the spatial velocity of the point.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
# l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
|
||||
p1 = self.xi1
|
||||
# p1 = Pose2().Adjoint(self.xi1)
|
||||
|
||||
sTl1 = l1Zl1
|
||||
p2 = sTl1.Adjoint(self.xi2)
|
||||
|
||||
sTl2 = compose(l1Zl1, l2Zl2)
|
||||
p3 = sTl2.Adjoint(self.xi3)
|
||||
|
||||
differential_twists = [p1, p2, p3]
|
||||
return np.stack(differential_twists, axis=1)
|
||||
|
||||
def plot(self, fignum, q):
|
||||
""" Plot arm.
|
||||
Takes figure number, and numpy array of joint angles, in radians.
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
p1 = sXl1.translation()
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
|
||||
|
||||
def plot_line(p, g, color):
|
||||
q = g.translation()
|
||||
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], color)
|
||||
return q
|
||||
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
|
||||
p2 = plot_line(p1, sTl2, 'r-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl2)
|
||||
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
|
||||
p3 = plot_line(p2, sTl3, 'g-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl3)
|
||||
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
sTt = compose(sTl3, l3Zl3, l3Xt)
|
||||
plot_line(p3, sTt, 'b-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTt)
|
||||
|
||||
|
||||
# Create common example configurations.
|
||||
Q0 = vector3(0, 0, 0)
|
||||
Q1 = np.radians(vector3(-30, -45, -90))
|
||||
Q2 = np.radians(vector3(-90, 90, 0))
|
||||
|
||||
|
||||
class TestPose2SLAMExample(GtsamTestCase):
|
||||
"""Unit tests for functions used below."""
|
||||
|
||||
def setUp(self):
|
||||
self.arm = ThreeLinkArm()
|
||||
|
||||
def assertPose2Equals(self, actual, expected, tol=1e-2):
|
||||
"""Helper function that prints out actual and expected if not equal."""
|
||||
equal = actual.equals(expected, tol)
|
||||
if not equal:
|
||||
raise self.failureException(
|
||||
"Poses are not equal:\n{}!={}".format(actual, expected))
|
||||
|
||||
def test_fk_arm(self):
|
||||
"""Make sure forward kinematics is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.fk(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.fk(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
def test_con_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.con(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.con(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_poe_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.poe(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.poe(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_ik(self):
|
||||
"""Check iterative inverse kinematics function."""
|
||||
# at rest
|
||||
actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
|
||||
np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
|
||||
|
||||
# -30, -45, -90
|
||||
sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
|
||||
actual = self.arm.ik(sTt_desired)
|
||||
self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
|
||||
np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
|
||||
|
||||
def test_manipulator_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array(
|
||||
[[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
|
||||
def run_example():
|
||||
""" Use trajectory interpolation and then trajectory tracking a la Murray
|
||||
to move a 3-link arm on a straight line.
|
||||
"""
|
||||
# Create arm
|
||||
arm = ThreeLinkArm()
|
||||
|
||||
# Get initial pose using forward kinematics
|
||||
q = np.radians(vector3(30, -30, 45))
|
||||
sTt_initial = arm.fk(q)
|
||||
|
||||
# Create interpolated trajectory in task space to desired goal pose
|
||||
sTt_goal = Pose2(2.4, 4.3, math.radians(0))
|
||||
poses = trajectory(sTt_initial, sTt_goal, 50)
|
||||
|
||||
# Setup figure and plot initial pose
|
||||
fignum = 0
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
axes.set_xlim(-5, 5)
|
||||
axes.set_ylim(0, 10)
|
||||
gtsam_plot.plot_pose2(fignum, arm.fk(q))
|
||||
|
||||
# For all poses in interpolated trajectory, calculate dq to move to next pose.
|
||||
# We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
|
||||
for pose in poses:
|
||||
sTt = arm.fk(q)
|
||||
error = delta(sTt, pose)
|
||||
J = arm.jacobian(q)
|
||||
q += np.dot(np.linalg.inv(J), error)
|
||||
arm.plot(fignum, q)
|
||||
plt.pause(0.01)
|
||||
|
||||
plt.pause(10)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run_example()
|
||||
unittest.main()
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
||||
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.symbol_shorthand import X, L
|
||||
|
||||
# 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)
|
||||
|
||||
# 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))
|
||||
|
||||
# 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))
|
||||
|
||||
# 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 graph
|
||||
print("Factor Graph:\n{}".format(graph))
|
||||
|
||||
# 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))
|
||||
|
||||
# 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, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
|
||||
print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))
|
||||
|
|
@ -1,96 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
||||
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
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
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
# 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()
|
||||
|
||||
# 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))
|
||||
|
||||
# 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
|
||||
|
||||
# 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))
|
||||
|
||||
# 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)))
|
||||
|
||||
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()
|
||||
|
|
@ -1,88 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
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
|
||||
"""
|
||||
# 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
|
||||
from gtsam.utils import plot
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.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(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()
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
"""
|
||||
* @file Pose3SLAMExample_initializePose3.cpp
|
||||
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
|
||||
* Pose3 using InitializePose3
|
||||
* @date Jan 17, 2019
|
||||
* @author Vikrant Shah based on CPP example by Luca Carlone
|
||||
"""
|
||||
# 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
|
||||
from gtsam.utils import plot
|
||||
|
||||
|
||||
def vector6(x, y, z, a, b, c):
|
||||
"""Create 6d double numpy array."""
|
||||
return np.array([x, y, z, a, b, c], dtype=np.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()
|
||||
|
||||
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
|
||||
else args.input
|
||||
|
||||
is3D = True
|
||||
graph, initial = gtsam.readG2o(g2oFile, is3D)
|
||||
|
||||
# Add Prior on the first key
|
||||
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
|
||||
1e-4, 1e-4, 1e-4))
|
||||
|
||||
print("Adding prior to g2o file ")
|
||||
firstKey = initial.keys()[0]
|
||||
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
|
||||
|
||||
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("initial error = ", graph.error(initial))
|
||||
print("final error = ", graph.error(result))
|
||||
|
||||
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()
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
Initialize PoseSLAM with Chordal init
|
||||
Author: Luca Carlone, Frank Dellaert (python port)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
# Read graph from file
|
||||
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
|
||||
|
||||
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))
|
||||
|
||||
# Initializing Pose3 - chordal relaxation"
|
||||
initialization = gtsam.InitializePose3.initialize(graph)
|
||||
|
||||
print(initialization)
|
||||
|
|
@ -1,141 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A script validating the Preintegration of IMU measurements
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam.utils.plot import plot_pose3
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
IMU_FIG = 1
|
||||
POSES_FIG = 2
|
||||
|
||||
|
||||
class PreintegrationExample(object):
|
||||
|
||||
@staticmethod
|
||||
def defaultParams(g):
|
||||
"""Create default parameters with Z *up* and realistic noise parameters"""
|
||||
params = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||
kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
|
||||
kAccelSigma = 0.1 / 60 # 10 cm VRW
|
||||
params.setGyroscopeCovariance(
|
||||
kGyroSigma ** 2 * np.identity(3, np.float))
|
||||
params.setAccelerometerCovariance(
|
||||
kAccelSigma ** 2 * np.identity(3, np.float))
|
||||
params.setIntegrationCovariance(
|
||||
0.0000001 ** 2 * np.identity(3, np.float))
|
||||
return params
|
||||
|
||||
def __init__(self, twist=None, bias=None, dt=1e-2):
|
||||
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
|
||||
|
||||
# setup interactive plotting
|
||||
plt.ion()
|
||||
|
||||
# Setup loop as default scenario
|
||||
if twist is not None:
|
||||
(W, V) = twist
|
||||
else:
|
||||
# default = loop with forward velocity 2m/s, while pitching up
|
||||
# with angular velocity 30 degree/sec (negative in FLU)
|
||||
W = np.array([0, -math.radians(30), 0])
|
||||
V = np.array([2, 0, 0])
|
||||
|
||||
self.scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
self.dt = dt
|
||||
|
||||
self.maxDim = 5
|
||||
self.labels = list('xyz')
|
||||
self.colors = list('rgb')
|
||||
|
||||
# Create runner
|
||||
self.g = 10 # simple gravity constant
|
||||
self.params = self.defaultParams(self.g)
|
||||
|
||||
if bias is not None:
|
||||
self.actualBias = bias
|
||||
else:
|
||||
accBias = np.array([0, 0.1, 0])
|
||||
gyroBias = np.array([0, 0, 0])
|
||||
self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
|
||||
|
||||
self.runner = gtsam.ScenarioRunner(
|
||||
self.scenario, self.params, self.dt, self.actualBias)
|
||||
|
||||
fig, self.axes = plt.subplots(4, 3)
|
||||
fig.set_tight_layout(True)
|
||||
|
||||
def plotImu(self, t, measuredOmega, measuredAcc):
|
||||
plt.figure(IMU_FIG)
|
||||
|
||||
# plot angular velocity
|
||||
omega_b = self.scenario.omega_b(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
ax = self.axes[0][i]
|
||||
ax.scatter(t, omega_b[i], color='k', marker='.')
|
||||
ax.scatter(t, measuredOmega[i], color=color, marker='.')
|
||||
ax.set_xlabel('angular velocity ' + label)
|
||||
|
||||
# plot acceleration in nav
|
||||
acceleration_n = self.scenario.acceleration_n(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
ax = self.axes[1][i]
|
||||
ax.scatter(t, acceleration_n[i], color=color, marker='.')
|
||||
ax.set_xlabel('acceleration in nav ' + label)
|
||||
|
||||
# plot acceleration in body
|
||||
acceleration_b = self.scenario.acceleration_b(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
ax = self.axes[2][i]
|
||||
ax.scatter(t, acceleration_b[i], color=color, marker='.')
|
||||
ax.set_xlabel('acceleration in body ' + label)
|
||||
|
||||
# plot actual specific force, as well as corrupted
|
||||
actual = self.runner.actualSpecificForce(t)
|
||||
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||
ax = self.axes[3][i]
|
||||
ax.scatter(t, actual[i], color='k', marker='.')
|
||||
ax.scatter(t, measuredAcc[i], color=color, marker='.')
|
||||
ax.set_xlabel('specific force ' + label)
|
||||
|
||||
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
|
||||
# plot ground truth pose, as well as prediction from integrated IMU measurements
|
||||
actualPose = self.scenario.pose(t)
|
||||
plot_pose3(POSES_FIG, actualPose, 0.3)
|
||||
t = actualPose.translation()
|
||||
self.maxDim = max([max(np.abs(t)), self.maxDim])
|
||||
ax = plt.gca()
|
||||
ax.set_xlim3d(-self.maxDim, self.maxDim)
|
||||
ax.set_ylim3d(-self.maxDim, self.maxDim)
|
||||
ax.set_zlim3d(-self.maxDim, self.maxDim)
|
||||
|
||||
plt.pause(time_interval)
|
||||
|
||||
def run(self, T=12):
|
||||
# simulate the loop
|
||||
for i, t in enumerate(np.arange(0, T, self.dt)):
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||
if i % 25 == 0:
|
||||
self.plotImu(t, measuredOmega, measuredAcc)
|
||||
self.plotGroundTruthPose(t)
|
||||
pim = self.runner.integrate(t, self.actualBias, True)
|
||||
predictedNavState = self.runner.predict(pim, self.actualBias)
|
||||
plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
PreintegrationExample().run()
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
# Porting Progress
|
||||
|
||||
| C++ Example Name | Ported |
|
||||
|-------------------------------------------------------|--------|
|
||||
| CameraResectioning | |
|
||||
| CreateSFMExampleData | |
|
||||
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python |
|
||||
| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python |
|
||||
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python |
|
||||
| ImuFactorExample2 | X |
|
||||
| ImuFactorsExample | |
|
||||
| ISAM2Example_SmartFactor | |
|
||||
| ISAM2_SmartFactorStereo_IMU | |
|
||||
| LocalizationExample | impossible? |
|
||||
| METISOrderingExample | |
|
||||
| OdometryExample | X |
|
||||
| PlanarSLAMExample | X |
|
||||
| Pose2SLAMExample | X |
|
||||
| Pose2SLAMExampleExpressions | |
|
||||
| Pose2SLAMExample_g2o | X |
|
||||
| Pose2SLAMExample_graph | |
|
||||
| Pose2SLAMExample_graphviz | |
|
||||
| Pose2SLAMExample_lago | lago not yet exposed through Python |
|
||||
| Pose2SLAMStressTest | |
|
||||
| Pose2SLAMwSPCG | |
|
||||
| Pose3SLAMExample_changeKeys | |
|
||||
| Pose3SLAMExampleExpressions_BearingRangeWithTransform | |
|
||||
| Pose3SLAMExample_g2o | X |
|
||||
| Pose3SLAMExample_initializePose3Chordal | |
|
||||
| Pose3SLAMExample_initializePose3Gradient | |
|
||||
| RangeISAMExample_plaza2 | |
|
||||
| SelfCalibrationExample | |
|
||||
| SFMExample_bal_COLAMD_METIS | |
|
||||
| SFMExample_bal | |
|
||||
| SFMExample | X |
|
||||
| SFMExampleExpressions_bal | |
|
||||
| SFMExampleExpressions | |
|
||||
| SFMExample_SmartFactor | |
|
||||
| SFMExample_SmartFactorPCG | |
|
||||
| SimpleRotation | X |
|
||||
| SolverComparer | |
|
||||
| StereoVOExample | |
|
||||
| StereoVOExample_large | |
|
||||
| TimeTBB | |
|
||||
| UGM_chain | discrete functionality not yet exposed |
|
||||
| UGM_small | discrete functionality not yet exposed |
|
||||
| VisualISAM2Example | X |
|
||||
| VisualISAMExample | X |
|
||||
|
||||
Extra Examples (with no C++ equivalent)
|
||||
- PlanarManipulatorExample
|
||||
- SFMData
|
||||
|
|
@ -1,122 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A structure-from-motion problem on a simulated dataset
|
||||
"""
|
||||
from __future__ import print_function
|
||||
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam import symbol_shorthand
|
||||
L = symbol_shorthand.L
|
||||
X = symbol_shorthand.X
|
||||
|
||||
from gtsam.examples import SFMdata
|
||||
from gtsam import (Cal3_S2, DoglegOptimizer,
|
||||
GenericProjectionFactorCal3_S2, Marginals,
|
||||
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
|
||||
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
|
||||
from gtsam.utils import plot
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
|
||||
Each variable in the system (poses and landmarks) must be identified with a unique key.
|
||||
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
|
||||
Here we will use Symbols
|
||||
|
||||
In GTSAM, measurement functions are represented as 'factors'. Several common factors
|
||||
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
|
||||
Here we will use Projection factors to model the camera's landmark observations.
|
||||
Also, we will initialize the robot at some location using a Prior factor.
|
||||
|
||||
When the factors are created, we will add them to a Factor Graph. As the factors we are using
|
||||
are nonlinear factors, we will need a Nonlinear Factor Graph.
|
||||
|
||||
Finally, once all of the factors have been added to our factor graph, we will want to
|
||||
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
|
||||
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
|
||||
trust-region method known as Powell's Degleg
|
||||
|
||||
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
|
||||
nonlinear functions around an initial linearization point, then solve the linear system
|
||||
to update the linearization point. This happens repeatedly until the solver converges
|
||||
to a consistent set of variable values. This requires us to specify an initial guess
|
||||
for each variable, held in a Values container.
|
||||
"""
|
||||
|
||||
# Define the camera calibration parameters
|
||||
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
|
||||
|
||||
# Define the camera observation noise model
|
||||
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
|
||||
|
||||
# Create the set of ground-truth landmarks
|
||||
points = SFMdata.createPoints()
|
||||
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
|
||||
# Create a factor graph
|
||||
graph = NonlinearFactorGraph()
|
||||
|
||||
# Add a prior on pose x1. This indirectly specifies where the origin is.
|
||||
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
|
||||
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
|
||||
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
|
||||
graph.push_back(factor)
|
||||
|
||||
# Simulated measurements from each camera pose, adding them to the factor graph
|
||||
for i, pose in enumerate(poses):
|
||||
camera = PinholeCameraCal3_S2(pose, K)
|
||||
for j, point in enumerate(points):
|
||||
measurement = camera.project(point)
|
||||
factor = GenericProjectionFactorCal3_S2(
|
||||
measurement, measurement_noise, X(i), L(j), K)
|
||||
graph.push_back(factor)
|
||||
|
||||
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
|
||||
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
|
||||
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
|
||||
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
factor = PriorFactorPoint3(L(0), points[0], point_noise)
|
||||
graph.push_back(factor)
|
||||
graph.print_('Factor Graph:\n')
|
||||
|
||||
# Create the data structure to hold the initial estimate to the solution
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initial_estimate = Values()
|
||||
for i, pose in enumerate(poses):
|
||||
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
|
||||
initial_estimate.insert(X(i), transformed_pose)
|
||||
for j, point in enumerate(points):
|
||||
transformed_point = point + 0.1*np.random.randn(3)
|
||||
initial_estimate.insert(L(j), transformed_point)
|
||||
initial_estimate.print_('Initial Estimates:\n')
|
||||
|
||||
# Optimize the graph and print results
|
||||
params = gtsam.DoglegParams()
|
||||
params.setVerbosity('TERMINATION')
|
||||
optimizer = DoglegOptimizer(graph, initial_estimate, params)
|
||||
print('Optimizing:')
|
||||
result = optimizer.optimize()
|
||||
result.print_('Final results:\n')
|
||||
print('initial error = {}'.format(graph.error(initial_estimate)))
|
||||
print('final error = {}'.format(graph.error(result)))
|
||||
|
||||
marginals = Marginals(graph, result)
|
||||
plot.plot_3d_points(1, result, marginals=marginals)
|
||||
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
|
||||
plot.set_axes_equal(1)
|
||||
plt.show()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
"""
|
||||
A structure-from-motion example with landmarks
|
||||
- The landmarks form a 10 meter cube
|
||||
- The robot rotates around the landmarks, always facing towards the cube
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def createPoints():
|
||||
# Create the set of ground-truth landmarks
|
||||
points = [gtsam.Point3(10.0, 10.0, 10.0),
|
||||
gtsam.Point3(-10.0, 10.0, 10.0),
|
||||
gtsam.Point3(-10.0, -10.0, 10.0),
|
||||
gtsam.Point3(10.0, -10.0, 10.0),
|
||||
gtsam.Point3(10.0, 10.0, -10.0),
|
||||
gtsam.Point3(-10.0, 10.0, -10.0),
|
||||
gtsam.Point3(-10.0, -10.0, -10.0),
|
||||
gtsam.Point3(10.0, -10.0, -10.0)]
|
||||
return points
|
||||
|
||||
|
||||
def createPoses(K):
|
||||
# Create the set of ground-truth poses
|
||||
radius = 40.0
|
||||
height = 10.0
|
||||
angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
|
||||
up = gtsam.Point3(0, 0, 1)
|
||||
target = gtsam.Point3(0, 0, 0)
|
||||
poses = []
|
||||
for theta in angles:
|
||||
position = gtsam.Point3(radius*np.cos(theta),
|
||||
radius*np.sin(theta),
|
||||
height)
|
||||
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
|
||||
poses.append(camera.pose())
|
||||
return poses
|
||||
|
|
@ -1,84 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
This example will perform a relatively trivial optimization on
|
||||
a single variable with a single factor.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import gtsam
|
||||
from gtsam.symbol_shorthand import X
|
||||
|
||||
def main():
|
||||
"""
|
||||
Step 1: Create a factor to express a unary constraint
|
||||
|
||||
The "prior" in this case is the measurement from a sensor,
|
||||
with a model of the noise on the measurement.
|
||||
|
||||
The "Key" created here is a label used to associate parts of the
|
||||
state (stored in "RotValues") with particular factors. They require
|
||||
an index to allow for lookup, and should be unique.
|
||||
|
||||
In general, creating a factor requires:
|
||||
- A key or set of keys labeling the variables that are acted upon
|
||||
- A measurement value
|
||||
- A measurement model with the correct dimensionality for the factor
|
||||
"""
|
||||
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
|
||||
prior.print_('goal angle')
|
||||
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
|
||||
key = X(1)
|
||||
factor = gtsam.PriorFactorRot2(key, prior, model)
|
||||
|
||||
"""
|
||||
Step 2: Create a graph container and add the factor to it
|
||||
|
||||
Before optimizing, all factors need to be added to a Graph container,
|
||||
which provides the necessary top-level functionality for defining a
|
||||
system of constraints.
|
||||
|
||||
In this case, there is only one factor, but in a practical scenario,
|
||||
many more factors would be added.
|
||||
"""
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
graph.push_back(factor)
|
||||
graph.print_('full graph')
|
||||
|
||||
"""
|
||||
Step 3: Create an initial estimate
|
||||
|
||||
An initial estimate of the solution for the system is necessary to
|
||||
start optimization. This system state is the "Values" instance,
|
||||
which is similar in structure to a dictionary, in that it maps
|
||||
keys (the label created in step 1) to specific values.
|
||||
|
||||
The initial estimate provided to optimization will be used as
|
||||
a linearization point for optimization, so it is important that
|
||||
all of the variables in the graph have a corresponding value in
|
||||
this structure.
|
||||
"""
|
||||
initial = gtsam.Values()
|
||||
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
|
||||
initial.print_('initial estimate')
|
||||
|
||||
"""
|
||||
Step 4: Optimize
|
||||
|
||||
After formulating the problem with a graph of constraints
|
||||
and an initial estimate, executing optimization is as simple
|
||||
as calling a general optimization function with the graph and
|
||||
initial estimate. This will yield a new RotValues structure
|
||||
with the final state of the optimization.
|
||||
"""
|
||||
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
|
||||
result.print_('final result')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,153 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
An example of running visual SLAM using iSAM2.
|
||||
Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import L, X
|
||||
from gtsam.examples import SFMdata
|
||||
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
|
||||
|
||||
|
||||
def visual_ISAM2_plot(result):
|
||||
"""
|
||||
VisualISAMPlot plots current state of ISAM2 object
|
||||
Author: Ellon Paiva
|
||||
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
|
||||
"""
|
||||
|
||||
# Declare an id for the figure
|
||||
fignum = 0
|
||||
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plt.cla()
|
||||
|
||||
# Plot points
|
||||
# Can't use data because current frame might not see all points
|
||||
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
|
||||
# gtsam.plot_3d_points(result, [], marginals)
|
||||
gtsam_plot.plot_3d_points(fignum, result, 'rx')
|
||||
|
||||
# Plot cameras
|
||||
i = 0
|
||||
while result.exists(X(i)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
gtsam_plot.plot_pose3(fignum, pose_i, 10)
|
||||
i += 1
|
||||
|
||||
# draw
|
||||
axes.set_xlim3d(-40, 40)
|
||||
axes.set_ylim3d(-40, 40)
|
||||
axes.set_zlim3d(-40, 40)
|
||||
plt.pause(1)
|
||||
|
||||
|
||||
def visual_ISAM2_example():
|
||||
plt.ion()
|
||||
|
||||
# Define the camera calibration parameters
|
||||
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
|
||||
|
||||
# Define the camera observation noise model
|
||||
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
|
||||
2, 1.0) # one pixel in u and v
|
||||
|
||||
# Create the set of ground-truth landmarks
|
||||
points = SFMdata.createPoints()
|
||||
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
|
||||
# Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
|
||||
# to maintain proper linearization and efficient variable ordering, iSAM2
|
||||
# performs partial relinearization/reordering at each step. A parameter
|
||||
# structure is available that allows the user to set various properties, such
|
||||
# as the relinearization threshold and type of linear solver. For this
|
||||
# example, we we set the relinearization threshold small so the iSAM2 result
|
||||
# will approach the batch result.
|
||||
parameters = gtsam.ISAM2Params()
|
||||
parameters.setRelinearizeThreshold(0.01)
|
||||
parameters.setRelinearizeSkip(1)
|
||||
isam = gtsam.ISAM2(parameters)
|
||||
|
||||
# Create a Factor Graph and Values to hold the new data
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
initial_estimate = gtsam.Values()
|
||||
|
||||
# Loop over the different poses, adding the observations to iSAM incrementally
|
||||
for i, pose in enumerate(poses):
|
||||
|
||||
# Add factors for each landmark observation
|
||||
for j, point in enumerate(points):
|
||||
camera = gtsam.PinholeCameraCal3_S2(pose, K)
|
||||
measurement = camera.project(point)
|
||||
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(
|
||||
measurement, measurement_noise, X(i), L(j), K))
|
||||
|
||||
# Add an initial guess for the current pose
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initial_estimate.insert(X(i), pose.compose(gtsam.Pose3(
|
||||
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
|
||||
|
||||
# If this is the first iteration, add a prior on the first pose to set the
|
||||
# coordinate frame and a prior on the first landmark to set the scale.
|
||||
# Also, as iSAM solves incrementally, we must wait until each is observed
|
||||
# at least twice before adding it to iSAM.
|
||||
if i == 0:
|
||||
# Add a prior on pose x0
|
||||
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array(
|
||||
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
|
||||
|
||||
# Add a prior on landmark l0
|
||||
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
graph.push_back(gtsam.PriorFactorPoint3(
|
||||
L(0), points[0], point_noise)) # add directly to graph
|
||||
|
||||
# Add initial guesses to all observed landmarks
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
for j, point in enumerate(points):
|
||||
initial_estimate.insert(L(j), gtsam.Point3(
|
||||
point[0]-0.25, point[1]+0.20, point[2]+0.15))
|
||||
else:
|
||||
# Update iSAM with the new factors
|
||||
isam.update(graph, initial_estimate)
|
||||
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
|
||||
# If accuracy is desired at the expense of time, update(*) can be called additional
|
||||
# times to perform multiple optimizer iterations every step.
|
||||
isam.update()
|
||||
current_estimate = isam.calculateEstimate()
|
||||
print("****************************************************")
|
||||
print("Frame", i, ":")
|
||||
for j in range(i + 1):
|
||||
print(X(j), ":", current_estimate.atPose3(X(j)))
|
||||
|
||||
for j in range(len(points)):
|
||||
print(L(j), ":", current_estimate.atPoint3(L(j)))
|
||||
|
||||
visual_ISAM2_plot(current_estimate)
|
||||
|
||||
# Clear the factor graph and values for the next iteration
|
||||
graph.resize(0)
|
||||
initial_estimate.clear()
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
visual_ISAM2_example()
|
||||
|
|
@ -1,105 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
A visualSLAM example for the structure-from-motion problem on a simulated dataset
|
||||
This version uses iSAM to solve the problem incrementally
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
import gtsam
|
||||
from gtsam.examples import SFMdata
|
||||
from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
|
||||
NonlinearFactorGraph, NonlinearISAM, Pose3,
|
||||
PriorFactorPoint3, PriorFactorPose3, Rot3,
|
||||
PinholeCameraCal3_S2, Values, Point3)
|
||||
from gtsam.symbol_shorthand import X, L
|
||||
|
||||
def main():
|
||||
"""
|
||||
A structure-from-motion example with landmarks
|
||||
- The landmarks form a 10 meter cube
|
||||
- The robot rotates around the landmarks, always facing towards the cube
|
||||
"""
|
||||
|
||||
# Define the camera calibration parameters
|
||||
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
|
||||
|
||||
# Define the camera observation noise model
|
||||
camera_noise = gtsam.noiseModel.Isotropic.Sigma(
|
||||
2, 1.0) # one pixel in u and v
|
||||
|
||||
# Create the set of ground-truth landmarks
|
||||
points = SFMdata.createPoints()
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
|
||||
# Create a NonlinearISAM object which will relinearize and reorder the variables
|
||||
# every "reorderInterval" updates
|
||||
isam = NonlinearISAM(reorderInterval=3)
|
||||
|
||||
# Create a Factor Graph and Values to hold the new data
|
||||
graph = NonlinearFactorGraph()
|
||||
initial_estimate = Values()
|
||||
|
||||
# Loop over the different poses, adding the observations to iSAM incrementally
|
||||
for i, pose in enumerate(poses):
|
||||
camera = PinholeCameraCal3_S2(pose, K)
|
||||
# Add factors for each landmark observation
|
||||
for j, point in enumerate(points):
|
||||
measurement = camera.project(point)
|
||||
factor = GenericProjectionFactorCal3_S2(
|
||||
measurement, camera_noise, X(i), L(j), K)
|
||||
graph.push_back(factor)
|
||||
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
|
||||
t=Point3(0.05, -0.10, 0.20))
|
||||
initial_xi = pose.compose(noise)
|
||||
|
||||
# Add an initial guess for the current pose
|
||||
initial_estimate.insert(X(i), initial_xi)
|
||||
|
||||
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
|
||||
# and a prior on the first landmark to set the scale
|
||||
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
|
||||
# adding it to iSAM.
|
||||
if i == 0:
|
||||
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
|
||||
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
|
||||
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
|
||||
graph.push_back(factor)
|
||||
|
||||
# Add a prior on landmark l0
|
||||
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
factor = PriorFactorPoint3(L(0), points[0], point_noise)
|
||||
graph.push_back(factor)
|
||||
|
||||
# Add initial guesses to all observed landmarks
|
||||
noise = np.array([-0.25, 0.20, 0.15])
|
||||
for j, point in enumerate(points):
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initial_lj = points[j] + noise
|
||||
initial_estimate.insert(L(j), initial_lj)
|
||||
else:
|
||||
# Update iSAM with the new factors
|
||||
isam.update(graph, initial_estimate)
|
||||
current_estimate = isam.estimate()
|
||||
print('*' * 50)
|
||||
print('Frame {}:'.format(i))
|
||||
current_estimate.print_('Current estimate: ')
|
||||
|
||||
# Clear the factor graph and values for the next iteration
|
||||
graph.resize(0)
|
||||
initial_estimate.clear()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
{include_boost}
|
||||
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/stl_bind.h>
|
||||
#include <pybind11/pybind11.h>
|
||||
#include "gtsam/base/serialization.h"
|
||||
#include "gtsam/nonlinear/utilities.h" // for RedirectCout.
|
||||
|
||||
{includes}
|
||||
#include <boost/serialization/export.hpp>
|
||||
|
||||
{boost_class_export}
|
||||
|
||||
{hoder_type}
|
||||
|
||||
#include "python/gtsam/preamble.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace py = pybind11;
|
||||
|
||||
PYBIND11_MODULE({module_name}, m_) {{
|
||||
m_.doc() = "pybind11 wrapper of {module_name}";
|
||||
|
||||
{wrapped_namespace}
|
||||
|
||||
#include "python/gtsam/specializations.h"
|
||||
|
||||
}}
|
||||
|
||||
|
|
@ -1 +0,0 @@
|
|||
from .gtsam.imuBias import *
|
||||
|
|
@ -1 +0,0 @@
|
|||
from .gtsam.noiseModel import *
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::BetweenFactor<gtsam::Pose3>>);
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
|
||||
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
|
||||
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
|
||||
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
|
||||
|
|
@ -1 +0,0 @@
|
|||
from .gtsam.symbol_shorthand import *
|
||||
|
|
@ -1,47 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
ScenarioRunner unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import math
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestScenarioRunner(GtsamTestCase):
|
||||
def setUp(self):
|
||||
self.g = 10 # simple gravity constant
|
||||
|
||||
def test_loop_runner(self):
|
||||
# Forward velocity 2m/s
|
||||
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
v = 2
|
||||
w = math.radians(6)
|
||||
W = np.array([0, -w, 0])
|
||||
V = np.array([v, 0, 0])
|
||||
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
|
||||
dt = 0.1
|
||||
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
|
||||
bias = gtsam.imuBias.ConstantBias()
|
||||
runner = gtsam.ScenarioRunner(
|
||||
scenario, params, dt, bias)
|
||||
|
||||
# Test specific force at time 0: a is pointing up
|
||||
t = 0.0
|
||||
a = w * v
|
||||
np.testing.assert_almost_equal(
|
||||
np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
"""
|
||||
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
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestCal3Unified(GtsamTestCase):
|
||||
|
||||
def test_Cal3Unified(self):
|
||||
K = gtsam.Cal3Unified()
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
|
||||
def test_retract(self):
|
||||
expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
|
||||
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
|
||||
K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240,
|
||||
1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
|
||||
d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
|
||||
actual = K.retract(d)
|
||||
self.gtsamAssertEquals(actual, expected)
|
||||
np.testing.assert_allclose(d, K.localCoordinates(actual))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
FrobeniusFactor unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-name-in-module, import-error, invalid-name
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
|
||||
ShonanFactor3, SOn)
|
||||
|
||||
id = SO4()
|
||||
v1 = np.array([0, 0, 0, 0.1, 0, 0])
|
||||
Q1 = SO4.Expmap(v1)
|
||||
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
|
||||
Q2 = SO4.Expmap(v2)
|
||||
|
||||
|
||||
class TestFrobeniusFactorSO4(unittest.TestCase):
|
||||
"""Test FrobeniusFactor factors."""
|
||||
|
||||
def test_frobenius_factor(self):
|
||||
"""Test creation of a factor that calculates the Frobenius norm."""
|
||||
factor = FrobeniusFactorSO4(1, 2)
|
||||
actual = factor.evaluateError(Q1, Q2)
|
||||
expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
def test_frobenius_between_factor(self):
|
||||
"""Test creation of a Frobenius BetweenFactor."""
|
||||
factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
|
||||
actual = factor.evaluateError(Q1, Q2)
|
||||
expected = np.zeros((16,))
|
||||
np.testing.assert_array_almost_equal(actual, expected)
|
||||
|
||||
def test_frobenius_wormhole_factor(self):
|
||||
"""Test creation of a factor that calculates Shonan error."""
|
||||
R1 = SO3.Expmap(v1[3:])
|
||||
R2 = SO3.Expmap(v2[3:])
|
||||
factor = ShonanFactor3(1, 2, Rot3(R1.between(R2).matrix()), p=4)
|
||||
I4 = SOn(4)
|
||||
Q1 = I4.retract(v1)
|
||||
Q2 = I4.retract(v2)
|
||||
actual = factor.evaluateError(Q1, Q2)
|
||||
expected = np.zeros((12,))
|
||||
np.testing.assert_array_almost_equal(actual, expected, decimal=4)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,92 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Unit tests for Linear Factor Graphs.
|
||||
Author: Frank Dellaert & Gerry Chen
|
||||
"""
|
||||
# pylint: disable=invalid-name, no-name-in-module, no-member
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.symbol_shorthand import X
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
def create_graph():
|
||||
"""Create a basic linear factor graph for testing"""
|
||||
graph = gtsam.GaussianFactorGraph()
|
||||
|
||||
x0 = X(0)
|
||||
x1 = X(1)
|
||||
x2 = X(2)
|
||||
|
||||
BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
|
||||
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
|
||||
|
||||
graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
|
||||
graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
|
||||
graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
|
||||
|
||||
return graph, (x0, x1, x2)
|
||||
|
||||
class TestGaussianFactorGraph(GtsamTestCase):
|
||||
"""Tests for Gaussian Factor Graphs."""
|
||||
|
||||
def test_fg(self):
|
||||
"""Test solving a linear factor graph"""
|
||||
graph, X = create_graph()
|
||||
result = graph.optimize()
|
||||
|
||||
EXPECTEDX = [0, 1, 3]
|
||||
|
||||
# check solutions
|
||||
self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8)
|
||||
self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8)
|
||||
self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8)
|
||||
|
||||
def test_convertNonlinear(self):
|
||||
"""Test converting a linear factor graph to a nonlinear one"""
|
||||
graph, X = create_graph()
|
||||
|
||||
EXPECTEDM = [1, 2, 3]
|
||||
|
||||
# create nonlinear factor graph for marginalization
|
||||
nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph)
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values())
|
||||
nlresult = optimizer.optimizeSafely()
|
||||
|
||||
# marginalize
|
||||
marginals = gtsam.Marginals(nfg, nlresult)
|
||||
m = [marginals.marginalCovariance(x) for x in X]
|
||||
|
||||
# check linear marginalizations
|
||||
self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
|
||||
self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
|
||||
self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
|
||||
|
||||
def test_linearMarginalization(self):
|
||||
"""Marginalize a linear factor graph"""
|
||||
graph, X = create_graph()
|
||||
result = graph.optimize()
|
||||
|
||||
EXPECTEDM = [1, 2, 3]
|
||||
|
||||
# linear factor graph marginalize
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
m = [marginals.marginalCovariance(x) for x in X]
|
||||
|
||||
# check linear marginalizations
|
||||
self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
|
||||
self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
|
||||
self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -1,92 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
JacobianFactor unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestJacobianFactor(GtsamTestCase):
|
||||
|
||||
def test_eliminate(self):
|
||||
# Recommended way to specify a matrix (see cython/README)
|
||||
Ax2 = np.array(
|
||||
[[-5., 0.],
|
||||
[+0., -5.],
|
||||
[10., 0.],
|
||||
[+0., 10.]], order='F')
|
||||
|
||||
# This is good too
|
||||
Al1 = np.array(
|
||||
[[5, 0],
|
||||
[0, 5],
|
||||
[0, 0],
|
||||
[0, 0]], dtype=float, order = 'F')
|
||||
|
||||
# Not recommended for performance reasons, but should still work
|
||||
# as the wrapper should convert it to the correct type and storage order
|
||||
Ax1 = np.array(
|
||||
[[0, 0], # f4
|
||||
[0, 0], # f4
|
||||
[-10, 0], # f2
|
||||
[0, -10]]) # f2
|
||||
|
||||
x2 = 1
|
||||
l1 = 2
|
||||
x1 = 3
|
||||
|
||||
# the RHS
|
||||
b2 = np.array([-1., 1.5, 2., -1.])
|
||||
sigmas = np.array([1., 1., 1., 1.])
|
||||
model4 = gtsam.noiseModel.Diagonal.Sigmas(sigmas)
|
||||
combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
|
||||
|
||||
# eliminate the first variable (x2) in the combined factor, destructive
|
||||
# !
|
||||
ord = gtsam.Ordering()
|
||||
ord.push_back(x2)
|
||||
actualCG, lf = combined.eliminate(ord)
|
||||
|
||||
# create expected Conditional Gaussian
|
||||
R11 = np.array([[11.1803, 0.00],
|
||||
[0.00, 11.1803]])
|
||||
S12 = np.array([[-2.23607, 0.00],
|
||||
[+0.00, -2.23607]])
|
||||
S13 = np.array([[-8.94427, 0.00],
|
||||
[+0.00, -8.94427]])
|
||||
d = np.array([2.23607, -1.56525])
|
||||
expectedCG = gtsam.GaussianConditional(
|
||||
x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2))
|
||||
# check if the result matches
|
||||
self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
|
||||
|
||||
# the expected linear factor
|
||||
Bl1 = np.array([[4.47214, 0.00],
|
||||
[0.00, 4.47214]])
|
||||
|
||||
Bx1 = np.array(
|
||||
# x1
|
||||
[[-4.47214, 0.00],
|
||||
[+0.00, -4.47214]])
|
||||
|
||||
# the RHS
|
||||
b1 = np.array([0.0, 0.894427])
|
||||
|
||||
model2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([1., 1.]))
|
||||
expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
|
||||
|
||||
# check if the result matches the combined (reduced) factor
|
||||
self.gtsamAssertEquals(lf, expectedLF, 1e-4)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,83 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
KalmanFilter unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestKalmanFilter(GtsamTestCase):
|
||||
|
||||
def test_KalmanFilter(self):
|
||||
F = np.eye(2)
|
||||
B = np.eye(2)
|
||||
u = np.array([1.0, 0.0])
|
||||
modelQ = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
|
||||
Q = 0.01 * np.eye(2)
|
||||
H = np.eye(2)
|
||||
z1 = np.array([1.0, 0.0])
|
||||
z2 = np.array([2.0, 0.0])
|
||||
z3 = np.array([3.0, 0.0])
|
||||
modelR = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
|
||||
R = 0.01 * np.eye(2)
|
||||
|
||||
# Create the set of expected output TestValues
|
||||
expected0 = np.array([0.0, 0.0])
|
||||
P00 = 0.01 * np.eye(2)
|
||||
|
||||
expected1 = np.array([1.0, 0.0])
|
||||
P01 = P00 + Q
|
||||
I11 = np.linalg.inv(P01) + np.linalg.inv(R)
|
||||
|
||||
expected2 = np.array([2.0, 0.0])
|
||||
P12 = np.linalg.inv(I11) + Q
|
||||
I22 = np.linalg.inv(P12) + np.linalg.inv(R)
|
||||
|
||||
expected3 = np.array([3.0, 0.0])
|
||||
P23 = np.linalg.inv(I22) + Q
|
||||
I33 = np.linalg.inv(P23) + np.linalg.inv(R)
|
||||
|
||||
# Create an KalmanFilter object
|
||||
KF = gtsam.KalmanFilter(n=2)
|
||||
|
||||
# Create the Kalman Filter initialization point
|
||||
x_initial = np.array([0.0, 0.0])
|
||||
P_initial = 0.01 * np.eye(2)
|
||||
|
||||
# Create an KF object
|
||||
state = KF.init(x_initial, P_initial)
|
||||
self.assertTrue(np.allclose(expected0, state.mean()))
|
||||
self.assertTrue(np.allclose(P00, state.covariance()))
|
||||
|
||||
# Run iteration 1
|
||||
state = KF.predict(state, F, B, u, modelQ)
|
||||
self.assertTrue(np.allclose(expected1, state.mean()))
|
||||
self.assertTrue(np.allclose(P01, state.covariance()))
|
||||
state = KF.update(state, H, z1, modelR)
|
||||
self.assertTrue(np.allclose(expected1, state.mean()))
|
||||
self.assertTrue(np.allclose(I11, state.information()))
|
||||
|
||||
# Run iteration 2
|
||||
state = KF.predict(state, F, B, u, modelQ)
|
||||
self.assertTrue(np.allclose(expected2, state.mean()))
|
||||
state = KF.update(state, H, z2, modelR)
|
||||
self.assertTrue(np.allclose(expected2, state.mean()))
|
||||
|
||||
# Run iteration 3
|
||||
state = KF.predict(state, F, B, u, modelQ)
|
||||
self.assertTrue(np.allclose(expected3, state.mean()))
|
||||
state = KF.update(state, H, z3, modelR)
|
||||
self.assertTrue(np.allclose(expected3, state.mean()))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,80 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
KarcherMeanFactor unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
|
||||
# pylint: disable=invalid-name, no-name-in-module, no-member
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
KEY = 0
|
||||
MODEL = gtsam.noiseModel.Unit.Create(3)
|
||||
|
||||
|
||||
def find_Karcher_mean_Rot3(rotations):
|
||||
"""Find the Karcher mean of given values."""
|
||||
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
|
||||
# No closed form solution.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
for R in rotations:
|
||||
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
|
||||
initial = gtsam.Values()
|
||||
initial.insert(KEY, gtsam.Rot3())
|
||||
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
|
||||
return result.atRot3(KEY)
|
||||
|
||||
|
||||
# Rot3 version
|
||||
R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
|
||||
|
||||
|
||||
class TestKarcherMean(GtsamTestCase):
|
||||
|
||||
def test_find(self):
|
||||
# Check that optimizing for Karcher mean (which minimizes Between distance)
|
||||
# gets correct result.
|
||||
rotations = {R, R.inverse()}
|
||||
expected = gtsam.Rot3()
|
||||
actual = find_Karcher_mean_Rot3(rotations)
|
||||
self.gtsamAssertEquals(expected, actual)
|
||||
|
||||
def test_factor(self):
|
||||
"""Check that the InnerConstraint factor leaves the mean unchanged."""
|
||||
# Make a graph with two variables, one between, and one InnerConstraint
|
||||
# The optimal result should satisfy the between, while moving the other
|
||||
# variable to make the mean the same as before.
|
||||
# Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
|
||||
# R*R*R, i.e. geodesic length is 3 rather than 2.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
R12 = R.compose(R.compose(R))
|
||||
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
|
||||
keys = gtsam.KeyVector()
|
||||
keys.append(1)
|
||||
keys.append(2)
|
||||
graph.add(gtsam.KarcherMeanFactorRot3(keys))
|
||||
|
||||
initial = gtsam.Values()
|
||||
initial.insert(1, R.inverse())
|
||||
initial.insert(2, R)
|
||||
expected = find_Karcher_mean_Rot3([R, R.inverse()])
|
||||
|
||||
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
|
||||
actual = find_Karcher_mean_Rot3(
|
||||
[result.atRot3(1), result.atRot3(2)])
|
||||
self.gtsamAssertEquals(expected, actual)
|
||||
self.gtsamAssertEquals(
|
||||
R12, result.atRot3(1).between(result.atRot3(2)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,64 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Localization unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestLocalizationExample(GtsamTestCase):
|
||||
|
||||
def test_LocalizationExample(self):
|
||||
# Create the graph (defined in pose2SLAM.h, derived from
|
||||
# NonlinearFactorGraph)
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add two odometry factors
|
||||
# create a measurement for both factors (the same in this case)
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
|
||||
graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
|
||||
# Add three "GPS" measurements
|
||||
# We use Pose2 Priors here with high variance on theta
|
||||
groundTruth = gtsam.Values()
|
||||
groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))
|
||||
groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0))
|
||||
groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0))
|
||||
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
|
||||
for i in range(3):
|
||||
graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
P = [None] * result.size()
|
||||
for i in range(0, result.size()):
|
||||
pose_i = result.atPose2(i)
|
||||
self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4)
|
||||
P[i] = marginals.marginalCovariance(i)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,83 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Unit tests for IMU testing scenarios.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, no-name-in-module
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
from gtsam import (DoglegOptimizer, DoglegParams,
|
||||
DummyPreconditionerParameters, GaussNewtonOptimizer,
|
||||
GaussNewtonParams, LevenbergMarquardtOptimizer,
|
||||
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
|
||||
PCGSolverParameters, Point2, PriorFactorPoint2, Values)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
KEY1 = 1
|
||||
KEY2 = 2
|
||||
|
||||
|
||||
class TestScenario(GtsamTestCase):
|
||||
def test_optimize(self):
|
||||
"""Do trivial test with three optimizer variants."""
|
||||
fg = NonlinearFactorGraph()
|
||||
model = gtsam.noiseModel.Unit.Create(2)
|
||||
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
|
||||
|
||||
# test error at minimum
|
||||
xstar = Point2(0, 0)
|
||||
optimal_values = Values()
|
||||
optimal_values.insert(KEY1, xstar)
|
||||
self.assertEqual(0.0, fg.error(optimal_values), 0.0)
|
||||
|
||||
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
||||
x0 = Point2(3, 3)
|
||||
initial_values = Values()
|
||||
initial_values.insert(KEY1, x0)
|
||||
self.assertEqual(9.0, fg.error(initial_values), 1e-3)
|
||||
|
||||
# optimize parameters
|
||||
ordering = Ordering()
|
||||
ordering.push_back(KEY1)
|
||||
|
||||
# Gauss-Newton
|
||||
gnParams = GaussNewtonParams()
|
||||
gnParams.setOrdering(ordering)
|
||||
actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
|
||||
self.assertAlmostEqual(0, fg.error(actual1))
|
||||
|
||||
# Levenberg-Marquardt
|
||||
lmParams = LevenbergMarquardtParams.CeresDefaults()
|
||||
lmParams.setOrdering(ordering)
|
||||
actual2 = LevenbergMarquardtOptimizer(
|
||||
fg, initial_values, lmParams).optimize()
|
||||
self.assertAlmostEqual(0, fg.error(actual2))
|
||||
|
||||
# Levenberg-Marquardt
|
||||
lmParams = LevenbergMarquardtParams.CeresDefaults()
|
||||
lmParams.setLinearSolverType("ITERATIVE")
|
||||
cgParams = PCGSolverParameters()
|
||||
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
|
||||
lmParams.setIterativeParams(cgParams)
|
||||
actual2 = LevenbergMarquardtOptimizer(
|
||||
fg, initial_values, lmParams).optimize()
|
||||
self.assertAlmostEqual(0, fg.error(actual2))
|
||||
|
||||
# Dogleg
|
||||
dlParams = DoglegParams()
|
||||
dlParams.setOrdering(ordering)
|
||||
actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
|
||||
self.assertAlmostEqual(0, fg.error(actual3))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Odometry unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestOdometryExample(GtsamTestCase):
|
||||
|
||||
def test_OdometryExample(self):
|
||||
# Create the graph (defined in pose2SLAM.h, derived from
|
||||
# NonlinearFactorGraph)
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add a Gaussian prior on pose x_1
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin
|
||||
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta
|
||||
# add directly to graph
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
|
||||
|
||||
# Add two odometry factors
|
||||
# create a measurement for both factors (the same in this case)
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
marginals.marginalCovariance(1)
|
||||
|
||||
# Check first pose equality
|
||||
pose_1 = result.atPose2(1)
|
||||
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,78 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
PlanarSLAM unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
from math import pi
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestPlanarSLAM(GtsamTestCase):
|
||||
|
||||
def test_PlanarSLAM(self):
|
||||
# Assumptions
|
||||
# - All values are axis aligned
|
||||
# - Robot poses are facing along the X axis (horizontal, to the right in images)
|
||||
# - We have full odometry for measurements
|
||||
# - The robot is on a grid, moving 2 meters each step
|
||||
|
||||
# Create graph container and add factors to it
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add prior
|
||||
# gaussian for prior
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# add directly to graph
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
|
||||
|
||||
# Add odometry
|
||||
# general noisemodel for odometry
|
||||
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
|
||||
# Add pose constraint
|
||||
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
P = marginals.marginalCovariance(1)
|
||||
|
||||
pose_1 = result.atPose2(1)
|
||||
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,32 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Pose2 unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import Pose2
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestPose2(GtsamTestCase):
|
||||
"""Test selected Pose2 methods."""
|
||||
|
||||
def test_adjoint(self):
|
||||
"""Test adjoint method."""
|
||||
xi = np.array([1, 2, 3])
|
||||
expected = np.dot(Pose2.adjointMap_(xi), xi)
|
||||
actual = Pose2.adjoint_(xi, xi)
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,76 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Pose2SLAM unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
from math import pi
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestPose2SLAMExample(GtsamTestCase):
|
||||
|
||||
def test_Pose2SLAMExample(self):
|
||||
# Assumptions
|
||||
# - All values are axis aligned
|
||||
# - Robot poses are facing along the X axis (horizontal, to the right in images)
|
||||
# - We have full odometry for measurements
|
||||
# - The robot is on a grid, moving 2 meters each step
|
||||
|
||||
# Create graph container and add factors to it
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add prior
|
||||
# gaussian for prior
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
# add directly to graph
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
|
||||
|
||||
# Add odometry
|
||||
# general noisemodel for odometry
|
||||
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
|
||||
|
||||
# Add pose constraint
|
||||
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
|
||||
|
||||
# Initialize to noisy points
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
|
||||
initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
|
||||
initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization with an ordering from
|
||||
# colamd
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
# Plot Covariance Ellipses
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
P = marginals.marginalCovariance(1)
|
||||
|
||||
pose_1 = result.atPose2(1)
|
||||
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Pose3 unit tests.
|
||||
Author: Frank Dellaert, Duy Nguyen Ta
|
||||
"""
|
||||
# pylint: disable=no-name-in-module
|
||||
import math
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import Point3, Pose3, Rot3
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestPose3(GtsamTestCase):
|
||||
"""Test selected Pose3 methods."""
|
||||
|
||||
def test_between(self):
|
||||
"""Test between method."""
|
||||
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
|
||||
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
|
||||
expected = T2.inverse().compose(T3)
|
||||
actual = T2.between(T3)
|
||||
self.gtsamAssertEquals(actual, expected, 1e-6)
|
||||
|
||||
def test_transform_to(self):
|
||||
"""Test transformTo method."""
|
||||
transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
|
||||
actual = transform.transformTo(Point3(3, 2, 10))
|
||||
expected = Point3(2, 1, 10)
|
||||
self.gtsamAssertEquals(actual, expected, 1e-6)
|
||||
|
||||
def test_range(self):
|
||||
"""Test range method."""
|
||||
l1 = Point3(1, 0, 0)
|
||||
l2 = Point3(1, 1, 0)
|
||||
x1 = Pose3()
|
||||
|
||||
xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
|
||||
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
|
||||
|
||||
# establish range is indeed zero
|
||||
self.assertEqual(1, x1.range(point=l1))
|
||||
|
||||
# establish range is indeed sqrt2
|
||||
self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
|
||||
|
||||
# establish range is indeed zero
|
||||
self.assertEqual(1, x1.range(pose=xl1))
|
||||
|
||||
# establish range is indeed sqrt2
|
||||
self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
|
||||
|
||||
def test_adjoint(self):
|
||||
"""Test adjoint method."""
|
||||
xi = np.array([1, 2, 3, 4, 5, 6])
|
||||
expected = np.dot(Pose3.adjointMap_(xi), xi)
|
||||
actual = Pose3.adjoint_(xi, xi)
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,60 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
PoseSLAM unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
from math import pi
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
from gtsam.utils.circlePose3 import *
|
||||
|
||||
|
||||
class TestPose3SLAMExample(GtsamTestCase):
|
||||
|
||||
def test_Pose3SLAMExample(self):
|
||||
# Create a hexagon of poses
|
||||
hexagon = circlePose3(6, 1.0)
|
||||
p0 = hexagon.atPose3(0)
|
||||
p1 = hexagon.atPose3(1)
|
||||
|
||||
# create a Pose graph with one equality constraint and one measurement
|
||||
fg = gtsam.NonlinearFactorGraph()
|
||||
fg.add(gtsam.NonlinearEqualityPose3(0, p0))
|
||||
delta = p0.between(p1)
|
||||
covariance = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180]))
|
||||
fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
|
||||
fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
|
||||
|
||||
# Create initial config
|
||||
initial = gtsam.Values()
|
||||
s = 0.10
|
||||
initial.insert(0, p0)
|
||||
initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1)))
|
||||
initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
|
||||
|
||||
# optimize
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial)
|
||||
result = optimizer.optimizeSafely()
|
||||
|
||||
pose_1 = result.atPose3(1)
|
||||
self.gtsamAssertEquals(pose_1, p1, 1e-4)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,61 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
PriorFactor unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestPriorFactor(GtsamTestCase):
|
||||
|
||||
def test_PriorFactor(self):
|
||||
values = gtsam.Values()
|
||||
|
||||
key = 5
|
||||
priorPose3 = gtsam.Pose3()
|
||||
model = gtsam.noiseModel.Unit.Create(6)
|
||||
factor = gtsam.PriorFactorPose3(key, priorPose3, model)
|
||||
values.insert(key, priorPose3)
|
||||
self.assertEqual(factor.error(values), 0)
|
||||
|
||||
key = 3
|
||||
priorVector = np.array([0., 0., 0.])
|
||||
model = gtsam.noiseModel.Unit.Create(3)
|
||||
factor = gtsam.PriorFactorVector(key, priorVector, model)
|
||||
values.insert(key, priorVector)
|
||||
self.assertEqual(factor.error(values), 0)
|
||||
|
||||
def test_AddPrior(self):
|
||||
"""
|
||||
Test adding prior factors directly to factor graph via the .addPrior method.
|
||||
"""
|
||||
# define factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# define and add Pose3 prior
|
||||
key = 5
|
||||
priorPose3 = gtsam.Pose3()
|
||||
model = gtsam.noiseModel.Unit.Create(6)
|
||||
graph.addPriorPose3(key, priorPose3, model)
|
||||
self.assertEqual(graph.size(), 1)
|
||||
|
||||
# define and add Vector prior
|
||||
key = 3
|
||||
priorVector = np.array([0., 0., 0.])
|
||||
model = gtsam.noiseModel.Unit.Create(3)
|
||||
graph.addPriorVector(key, priorVector, model)
|
||||
self.assertEqual(graph.size(), 2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,83 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
SFM unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.visual_data_generator as generator
|
||||
from gtsam import symbol
|
||||
from gtsam.noiseModel import Isotropic, Diagonal
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
from gtsam.symbol_shorthand import X, P
|
||||
|
||||
class TestSFMExample(GtsamTestCase):
|
||||
|
||||
def test_SFMExample(self):
|
||||
options = generator.Options()
|
||||
options.triangle = False
|
||||
options.nrCameras = 10
|
||||
|
||||
[data, truth] = generator.generate_data(options)
|
||||
|
||||
measurementNoiseSigma = 1.0
|
||||
pointNoiseSigma = 0.1
|
||||
poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
|
||||
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Add factors for all measurements
|
||||
measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma)
|
||||
for i in range(len(data.Z)):
|
||||
for k in range(len(data.Z[i])):
|
||||
j = data.J[i][k]
|
||||
graph.add(gtsam.GenericProjectionFactorCal3_S2(
|
||||
data.Z[i][k], measurementNoise,
|
||||
X(i), P(j), data.K))
|
||||
|
||||
posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
|
||||
graph.add(gtsam.PriorFactorPose3(X(0),
|
||||
truth.cameras[0].pose(), posePriorNoise))
|
||||
pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
|
||||
graph.add(gtsam.PriorFactorPoint3(P(0),
|
||||
truth.points[0], pointPriorNoise))
|
||||
|
||||
# Initial estimate
|
||||
initialEstimate = gtsam.Values()
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = truth.cameras[i].pose()
|
||||
initialEstimate.insert(X(i), pose_i)
|
||||
for j in range(len(truth.points)):
|
||||
point_j = truth.points[j]
|
||||
initialEstimate.insert(P(j), point_j)
|
||||
|
||||
# Optimization
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
for i in range(5):
|
||||
optimizer.iterate()
|
||||
result = optimizer.values()
|
||||
|
||||
# Marginalization
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
marginals.marginalCovariance(P(0))
|
||||
marginals.marginalCovariance(X(0))
|
||||
|
||||
# Check optimized results, should be equal to ground truth
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
|
||||
|
||||
for j in range(len(truth.points)):
|
||||
point_j = result.atPoint3(P(j))
|
||||
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
SO4 unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-name-in-module, import-error
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import SO4
|
||||
|
||||
I4 = SO4()
|
||||
v1 = np.array([0, 0, 0, .1, 0, 0])
|
||||
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
|
||||
Q1 = SO4.Expmap(v1)
|
||||
Q2 = SO4.Expmap(v2)
|
||||
|
||||
|
||||
class TestSO4(unittest.TestCase):
|
||||
"""Test selected SO4 methods."""
|
||||
|
||||
def test_constructor(self):
|
||||
"""Construct from matrix."""
|
||||
matrix = np.eye(4)
|
||||
so4 = SO4(matrix)
|
||||
self.assertIsInstance(so4, SO4)
|
||||
|
||||
def test_retract(self):
|
||||
"""Test retraction to manifold."""
|
||||
v = np.zeros((6,), np.float)
|
||||
actual = I4.retract(v)
|
||||
self.assertTrue(actual.equals(I4, 1e-9))
|
||||
|
||||
def test_local(self):
|
||||
"""Check localCoordinates for trivial case."""
|
||||
v0 = np.zeros((6,), np.float)
|
||||
actual = I4.localCoordinates(I4)
|
||||
np.testing.assert_array_almost_equal(actual, v0)
|
||||
|
||||
def test_compose(self):
|
||||
"""Check compose works in subgroup."""
|
||||
expected = SO4.Expmap(2*v1)
|
||||
actual = Q1.compose(Q1)
|
||||
self.assertTrue(actual.equals(expected, 1e-9))
|
||||
|
||||
def test_vec(self):
|
||||
"""Check wrapping of vec."""
|
||||
expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
|
||||
actual = I4.vec()
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,59 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Dynamic SO(n) unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-name-in-module, import-error
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import SOn
|
||||
|
||||
I4 = SOn(4)
|
||||
v1 = np.array([0, 0, 0, .1, 0, 0])
|
||||
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
|
||||
Q1 = I4.retract(v1)
|
||||
Q2 = I4.retract(v2)
|
||||
|
||||
|
||||
class TestSO4(unittest.TestCase):
|
||||
"""Test selected SOn methods."""
|
||||
|
||||
def test_constructor(self):
|
||||
"""Construct from matrix."""
|
||||
matrix = np.eye(4)
|
||||
so4 = SOn.FromMatrix(matrix)
|
||||
self.assertIsInstance(so4, SOn)
|
||||
|
||||
def test_retract(self):
|
||||
"""Test retraction to manifold."""
|
||||
v = np.zeros((6,), np.float)
|
||||
actual = I4.retract(v)
|
||||
self.assertTrue(actual.equals(I4, 1e-9))
|
||||
|
||||
def test_local(self):
|
||||
"""Check localCoordinates for trivial case."""
|
||||
v0 = np.zeros((6,), np.float)
|
||||
actual = I4.localCoordinates(I4)
|
||||
np.testing.assert_array_almost_equal(actual, v0)
|
||||
|
||||
def test_compose(self):
|
||||
"""Check compose works in subgroup."""
|
||||
expected = I4.retract(2*v1)
|
||||
actual = Q1.compose(Q1)
|
||||
self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
|
||||
|
||||
def test_vec(self):
|
||||
"""Check wrapping of vec."""
|
||||
expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
|
||||
actual = I4.vec()
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Scenario unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
|
||||
class TestScenario(GtsamTestCase):
|
||||
def setUp(self):
|
||||
pass
|
||||
|
||||
def test_loop(self):
|
||||
# Forward velocity 2m/s
|
||||
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
v = 2
|
||||
w = math.radians(6)
|
||||
W = np.array([0, -w, 0])
|
||||
V = np.array([v, 0, 0])
|
||||
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
|
||||
T = 30
|
||||
np.testing.assert_almost_equal(W, scenario.omega_b(T))
|
||||
np.testing.assert_almost_equal(V, scenario.velocity_b(T))
|
||||
np.testing.assert_almost_equal(
|
||||
np.cross(W, V), scenario.acceleration_b(T))
|
||||
|
||||
# R = v/w, so test if loop crests at 2*R
|
||||
R = v / w
|
||||
T30 = scenario.pose(T)
|
||||
xyz = T30.rotation().xyz()
|
||||
if xyz[0] < 0:
|
||||
xyz = -xyz
|
||||
np.testing.assert_almost_equal(
|
||||
np.array([math.pi, 0, math.pi]), xyz)
|
||||
self.gtsamAssertEquals(gtsam.Point3(
|
||||
0, 0, 2.0 * R), T30.translation(), 1e-9)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -1,45 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
SimpleCamera unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import math
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
K = Cal3_S2(625, 625, 0, 0, 0)
|
||||
|
||||
class TestSimpleCamera(GtsamTestCase):
|
||||
|
||||
def test_constructor(self):
|
||||
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
|
||||
camera = SimpleCamera(pose1, K)
|
||||
self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
|
||||
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
|
||||
|
||||
def test_level2(self):
|
||||
# Create a level camera, looking in Y-direction
|
||||
pose2 = Pose2(0.4,0.3,math.pi/2.0)
|
||||
camera = SimpleCamera.Level(K, pose2, 0.1)
|
||||
|
||||
# expected
|
||||
x = Point3(1,0,0)
|
||||
y = Point3(0,0,-1)
|
||||
z = Point3(0,1,0)
|
||||
wRc = Rot3(x,y,z)
|
||||
expected = Pose3(wRc,Point3(0.4,0.3,0.1))
|
||||
self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,82 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Stereo VO unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import symbol
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestStereoVOExample(GtsamTestCase):
|
||||
|
||||
def test_StereoVOExample(self):
|
||||
## Assumptions
|
||||
# - For simplicity this example is in the camera's coordinate frame
|
||||
# - X: right, Y: down, Z: forward
|
||||
# - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
|
||||
# - x1 is fixed with a constraint, x2 is initialized with noisy values
|
||||
# - No noise on measurements
|
||||
|
||||
## Create keys for variables
|
||||
x1 = symbol('x',1)
|
||||
x2 = symbol('x',2)
|
||||
l1 = symbol('l',1)
|
||||
l2 = symbol('l',2)
|
||||
l3 = symbol('l',3)
|
||||
|
||||
## Create graph container and add factors to it
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
## add a constraint on the starting pose
|
||||
first_pose = gtsam.Pose3()
|
||||
graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
|
||||
|
||||
## Create realistic calibration and measurement noise model
|
||||
# format: fx fy skew cx cy baseline
|
||||
K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
|
||||
stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
|
||||
|
||||
## Add measurements
|
||||
# pose 1
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
|
||||
|
||||
#pose 2
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K))
|
||||
graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
|
||||
|
||||
## Create initial estimate for camera poses and landmarks
|
||||
initialEstimate = gtsam.Values()
|
||||
initialEstimate.insert(x1, first_pose)
|
||||
# noisy estimate for pose 2
|
||||
initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1)))
|
||||
expected_l1 = gtsam.Point3( 1, 1, 5)
|
||||
initialEstimate.insert(l1, expected_l1)
|
||||
initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
|
||||
initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5))
|
||||
|
||||
## optimize
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
||||
result = optimizer.optimize()
|
||||
|
||||
## check equality for the first pose and point
|
||||
pose_x1 = result.atPose3(x1)
|
||||
self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
|
||||
|
||||
point_l1 = result.atPoint3(l1)
|
||||
self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,80 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Test Triangulation
|
||||
Author: Frank Dellaert & Fan Jiang (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam as g
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
|
||||
PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
|
||||
|
||||
class TestVisualISAMExample(GtsamTestCase):
|
||||
def test_TriangulationExample(self):
|
||||
# Some common constants
|
||||
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
|
||||
|
||||
# Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
|
||||
pose1 = Pose3(upright, Point3(0, 0, 1))
|
||||
camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
|
||||
|
||||
# create second camera 1 meter to the right of first camera
|
||||
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
|
||||
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
|
||||
|
||||
# landmark ~5 meters infront of camera
|
||||
landmark = Point3(5, 0.5, 1.2)
|
||||
|
||||
# 1. Project two landmarks into two cameras and triangulate
|
||||
z1 = camera1.project(landmark)
|
||||
z2 = camera2.project(landmark)
|
||||
|
||||
# twoPoses
|
||||
poses = Pose3Vector()
|
||||
measurements = Point2Vector()
|
||||
|
||||
poses.append(pose1)
|
||||
poses.append(pose2)
|
||||
measurements.append(z1)
|
||||
measurements.append(z2)
|
||||
|
||||
optimize = True
|
||||
rank_tol = 1e-9
|
||||
|
||||
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
|
||||
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
|
||||
|
||||
# 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
|
||||
measurements = Point2Vector()
|
||||
measurements.append(z1 - np.array([0.1, 0.5]))
|
||||
measurements.append(z2 - np.array([-0.2, 0.3]))
|
||||
|
||||
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
|
||||
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2)
|
||||
#
|
||||
# # two Poses with Bundler Calibration
|
||||
# bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
|
||||
# camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
|
||||
# camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
|
||||
#
|
||||
# z1 = camera1.project(landmark)
|
||||
# z2 = camera2.project(landmark)
|
||||
#
|
||||
# measurements = Point2Vector()
|
||||
# measurements.append(z1)
|
||||
# measurements.append(z2)
|
||||
#
|
||||
# triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
|
||||
# self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,85 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Values unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101, E0611
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
|
||||
Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestValues(GtsamTestCase):
|
||||
|
||||
def test_values(self):
|
||||
values = Values()
|
||||
E = EssentialMatrix(Rot3(), Unit3())
|
||||
tol = 1e-9
|
||||
|
||||
values.insert(0, Point2(0, 0))
|
||||
values.insert(1, Point3(0, 0, 0))
|
||||
values.insert(2, Rot2())
|
||||
values.insert(3, Pose2())
|
||||
values.insert(4, Rot3())
|
||||
values.insert(5, Pose3())
|
||||
values.insert(6, Cal3_S2())
|
||||
values.insert(7, Cal3DS2())
|
||||
values.insert(8, Cal3Bundler())
|
||||
values.insert(9, E)
|
||||
values.insert(10, imuBias.ConstantBias())
|
||||
|
||||
# Special cases for Vectors and Matrices
|
||||
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
|
||||
# floating point numbers in column-major (Fortran style) storage order,
|
||||
# whereas by default, numpy.array is in row-major order and the type is
|
||||
# in whatever the number input type is, e.g. np.array([1,2,3])
|
||||
# will have 'int' type.
|
||||
#
|
||||
# The wrapper will automatically fix the type and storage order for you,
|
||||
# but for performance reasons, it's recommended to specify the correct
|
||||
# type and storage order.
|
||||
# for vectors, the order is not important, but dtype still is
|
||||
vec = np.array([1., 2., 3.])
|
||||
values.insert(11, vec)
|
||||
mat = np.array([[1., 2.], [3., 4.]], order='F')
|
||||
values.insert(12, mat)
|
||||
# Test with dtype int and the default order='C'
|
||||
# This still works as the wrapper converts to the correct type and order for you
|
||||
# but is nornally not recommended!
|
||||
mat2 = np.array([[1, 2, ], [3, 5]])
|
||||
values.insert(13, mat2)
|
||||
|
||||
self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol)
|
||||
self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol)
|
||||
self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol)
|
||||
self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol)
|
||||
self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol)
|
||||
self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol)
|
||||
self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol)
|
||||
self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
|
||||
self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
|
||||
self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
|
||||
self.gtsamAssertEquals(values.atConstantBias(
|
||||
10), imuBias.ConstantBias(), tol)
|
||||
|
||||
# special cases for Vector and Matrix:
|
||||
actualVector = values.atVector(11)
|
||||
np.testing.assert_allclose(vec, actualVector, tol)
|
||||
actualMatrix = values.atMatrix(12)
|
||||
np.testing.assert_allclose(mat, actualMatrix, tol)
|
||||
actualMatrix2 = values.atMatrix(13)
|
||||
np.testing.assert_allclose(mat2, actualMatrix2, tol)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
visual_isam unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.visual_data_generator as generator
|
||||
import gtsam.utils.visual_isam as visual_isam
|
||||
from gtsam import symbol
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestVisualISAMExample(GtsamTestCase):
|
||||
|
||||
def test_VisualISAMExample(self):
|
||||
# Data Options
|
||||
options = generator.Options()
|
||||
options.triangle = False
|
||||
options.nrCameras = 20
|
||||
|
||||
# iSAM Options
|
||||
isamOptions = visual_isam.Options()
|
||||
isamOptions.hardConstraint = False
|
||||
isamOptions.pointPriors = False
|
||||
isamOptions.batchInitialization = True
|
||||
isamOptions.reorderInterval = 10
|
||||
isamOptions.alwaysRelinearize = False
|
||||
|
||||
# Generate data
|
||||
data, truth = generator.generate_data(options)
|
||||
|
||||
# Initialize iSAM with the first pose and points
|
||||
isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions)
|
||||
|
||||
# Main loop for iSAM: stepping through all poses
|
||||
for currentPose in range(nextPose, options.nrCameras):
|
||||
isam, result = visual_isam.step(data, isam, result, truth, currentPose)
|
||||
|
||||
for i in range(len(truth.cameras)):
|
||||
pose_i = result.atPose3(symbol('x', i))
|
||||
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
|
||||
|
||||
for j in range(len(truth.points)):
|
||||
point_j = result.atPoint3(symbol('l', j))
|
||||
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,45 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Unit tests for testing dataset access.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, no-name-in-module, no-member
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
from gtsam import BetweenFactorPose3
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestDataset(GtsamTestCase):
|
||||
"""Tests for datasets.h wrapper."""
|
||||
|
||||
def setUp(self):
|
||||
"""Get some common paths."""
|
||||
self.pose3_example_g2o_file = gtsam.findExampleDataFile(
|
||||
"pose3example.txt")
|
||||
|
||||
def test_readG2o3D(self):
|
||||
"""Test reading directly into factor graph."""
|
||||
is3D = True
|
||||
graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
|
||||
self.assertEqual(graph.size(), 6)
|
||||
self.assertEqual(initial.size(), 5)
|
||||
|
||||
def test_parse3Dfactors(self):
|
||||
"""Test parsing into data structure."""
|
||||
factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
|
||||
self.assertEqual(len(factors), 6)
|
||||
self.assertIsInstance(factors[0], BetweenFactorPose3)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Unit tests for Disjoint Set Forest.
|
||||
Author: Frank Dellaert & Varun Agrawal
|
||||
"""
|
||||
# pylint: disable=invalid-name, no-name-in-module, no-member
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestDSFMap(GtsamTestCase):
|
||||
"""Tests for DSFMap."""
|
||||
|
||||
def test_all(self):
|
||||
"""Test everything in DFSMap."""
|
||||
def key(index_pair):
|
||||
return index_pair.i(), index_pair.j()
|
||||
|
||||
dsf = gtsam.DSFMapIndexPair()
|
||||
pair1 = gtsam.IndexPair(1, 18)
|
||||
self.assertEqual(key(dsf.find(pair1)), key(pair1))
|
||||
pair2 = gtsam.IndexPair(2, 2)
|
||||
|
||||
# testing the merge feature of dsf
|
||||
dsf.merge(pair1, pair2)
|
||||
self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -1,88 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Unit tests for 3D SLAM initialization, using rotation relaxation.
|
||||
Author: Luca Carlone and Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101, E0611
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
x0, x1, x2, x3 = 0, 1, 2, 3
|
||||
|
||||
|
||||
class TestValues(GtsamTestCase):
|
||||
|
||||
def setUp(self):
|
||||
|
||||
model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||
|
||||
# We consider a small graph:
|
||||
# symbolic FG
|
||||
# x2 0 1
|
||||
# / | \ 1 2
|
||||
# / | \ 2 3
|
||||
# x3 | x1 2 0
|
||||
# \ | / 0 3
|
||||
# \ | /
|
||||
# x0
|
||||
#
|
||||
p0 = Point3(0, 0, 0)
|
||||
self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
|
||||
p1 = Point3(1, 2, 0)
|
||||
self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
|
||||
p2 = Point3(0, 2, 0)
|
||||
self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
|
||||
p3 = Point3(-1, 1, 0)
|
||||
self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
|
||||
|
||||
pose0 = Pose3(self.R0, p0)
|
||||
pose1 = Pose3(self.R1, p1)
|
||||
pose2 = Pose3(self.R2, p2)
|
||||
pose3 = Pose3(self.R3, p3)
|
||||
|
||||
g = NonlinearFactorGraph()
|
||||
g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
|
||||
g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
|
||||
g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
|
||||
g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
|
||||
g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
|
||||
g.add(gtsam.PriorFactorPose3(x0, pose0, model))
|
||||
self.graph = g
|
||||
|
||||
def test_buildPose3graph(self):
|
||||
pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
|
||||
|
||||
def test_orientations(self):
|
||||
pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
|
||||
initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
|
||||
|
||||
# comparison is up to M_PI, that's why we add some multiples of 2*M_PI
|
||||
self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
|
||||
self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
|
||||
self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6)
|
||||
self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6)
|
||||
|
||||
def test_initializePoses(self):
|
||||
g2oFile = gtsam.findExampleDataFile("pose3example-grid")
|
||||
is3D = True
|
||||
inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
|
||||
priorModel = gtsam.noiseModel.Unit.Create(6)
|
||||
inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
|
||||
|
||||
initial = gtsam.InitializePose3.initialize(inputGraph)
|
||||
# TODO(frank): very loose !!
|
||||
self.gtsamAssertEquals(initial, expectedValues, 0.1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,108 +0,0 @@
|
|||
"""
|
||||
Unit tests for optimization that logs to comet.ml.
|
||||
Author: Jing Wu and Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name
|
||||
|
||||
import sys
|
||||
if sys.version_info.major >= 3:
|
||||
from io import StringIO
|
||||
else:
|
||||
from cStringIO import StringIO
|
||||
|
||||
import unittest
|
||||
from datetime import datetime
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam import Rot3
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
from gtsam.utils.logging_optimizer import gtsam_optimize
|
||||
|
||||
KEY = 0
|
||||
MODEL = gtsam.noiseModel.Unit.Create(3)
|
||||
|
||||
|
||||
class TestOptimizeComet(GtsamTestCase):
|
||||
"""Check correct logging to comet.ml."""
|
||||
|
||||
def setUp(self):
|
||||
"""Set up a small Karcher mean optimization example."""
|
||||
# Grabbed from KarcherMeanFactor unit tests.
|
||||
R = Rot3.Expmap(np.array([0.1, 0, 0]))
|
||||
rotations = {R, R.inverse()} # mean is the identity
|
||||
self.expected = Rot3()
|
||||
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
for R in rotations:
|
||||
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
|
||||
initial = gtsam.Values()
|
||||
initial.insert(KEY, R)
|
||||
self.params = gtsam.GaussNewtonParams()
|
||||
self.optimizer = gtsam.GaussNewtonOptimizer(
|
||||
graph, initial, self.params)
|
||||
|
||||
self.lmparams = gtsam.LevenbergMarquardtParams()
|
||||
self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
|
||||
graph, initial, self.lmparams
|
||||
)
|
||||
|
||||
# setup output capture
|
||||
self.capturedOutput = StringIO()
|
||||
sys.stdout = self.capturedOutput
|
||||
|
||||
def tearDown(self):
|
||||
"""Reset print capture."""
|
||||
sys.stdout = sys.__stdout__
|
||||
|
||||
def test_simple_printing(self):
|
||||
"""Test with a simple hook."""
|
||||
|
||||
# Provide a hook that just prints
|
||||
def hook(_, error):
|
||||
print(error)
|
||||
|
||||
# Only thing we require from optimizer is an iterate method
|
||||
gtsam_optimize(self.optimizer, self.params, hook)
|
||||
|
||||
# Check that optimizing yields the identity.
|
||||
actual = self.optimizer.values()
|
||||
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
|
||||
|
||||
def test_lm_simple_printing(self):
|
||||
"""Make sure we are properly terminating LM"""
|
||||
def hook(_, error):
|
||||
print(error)
|
||||
|
||||
gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
|
||||
|
||||
actual = self.lmoptimizer.values()
|
||||
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
|
||||
|
||||
@unittest.skip("Not a test we want run every time, as needs comet.ml account")
|
||||
def test_comet(self):
|
||||
"""Test with a comet hook."""
|
||||
from comet_ml import Experiment
|
||||
comet = Experiment(project_name="Testing",
|
||||
auto_output_logging="native")
|
||||
comet.log_dataset_info(name="Karcher", path="shonan")
|
||||
comet.add_tag("GaussNewton")
|
||||
comet.log_parameter("method", "GaussNewton")
|
||||
time = datetime.now()
|
||||
comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " "
|
||||
+ str(time.hour)+":"+str(time.minute)+":"+str(time.second))
|
||||
|
||||
# I want to do some comet thing here
|
||||
def hook(optimizer, error):
|
||||
comet.log_metric("Karcher error",
|
||||
error, optimizer.iterations())
|
||||
|
||||
gtsam_optimize(self.optimizer, self.params, hook)
|
||||
comet.end()
|
||||
|
||||
actual = self.optimizer.values()
|
||||
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
import gtsam
|
||||
import math
|
||||
import numpy as np
|
||||
from math import pi
|
||||
|
||||
|
||||
def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'):
|
||||
"""
|
||||
circlePose3 generates a set of poses in a circle. This function
|
||||
returns those poses inside a gtsam.Values object, with sequential
|
||||
keys starting from 0. An optional character may be provided, which
|
||||
will be stored in the msb of each key (i.e. gtsam.Symbol).
|
||||
|
||||
We use aerospace/navlab convention, X forward, Y right, Z down
|
||||
First pose will be at (R,0,0)
|
||||
^y ^ X
|
||||
| |
|
||||
z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
|
||||
Vehicle at p0 is looking towards y axis (X-axis points towards world y)
|
||||
"""
|
||||
|
||||
values = gtsam.Values()
|
||||
theta = 0.0
|
||||
dtheta = 2 * pi / numPoses
|
||||
gRo = gtsam.Rot3(
|
||||
np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
|
||||
for i in range(numPoses):
|
||||
key = gtsam.symbol(symbolChar, i)
|
||||
gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0)
|
||||
oRi = gtsam.Rot3.Yaw(
|
||||
-theta) # negative yaw goes counterclockwise, with Z down !
|
||||
gTi = gtsam.Pose3(gRo.compose(oRi), gti)
|
||||
values.insert(key, gTi)
|
||||
theta = theta + dtheta
|
||||
return values
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
"""
|
||||
Optimization with logging via a hook.
|
||||
Author: Jing Wu and Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name
|
||||
|
||||
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
|
||||
import gtsam
|
||||
|
||||
|
||||
def optimize(optimizer, check_convergence, hook):
|
||||
""" Given an optimizer and a convergence check, iterate until convergence.
|
||||
After each iteration, hook(optimizer, error) is called.
|
||||
After the function, use values and errors to get the result.
|
||||
Arguments:
|
||||
optimizer (T): needs an iterate and an error function.
|
||||
check_convergence: T * float * float -> bool
|
||||
hook -- hook function to record the error
|
||||
"""
|
||||
# the optimizer is created with default values which incur the error below
|
||||
current_error = optimizer.error()
|
||||
hook(optimizer, current_error)
|
||||
|
||||
# Iterative loop
|
||||
while True:
|
||||
# Do next iteration
|
||||
optimizer.iterate()
|
||||
new_error = optimizer.error()
|
||||
hook(optimizer, new_error)
|
||||
if check_convergence(optimizer, current_error, new_error):
|
||||
return
|
||||
current_error = new_error
|
||||
|
||||
|
||||
def gtsam_optimize(optimizer,
|
||||
params,
|
||||
hook):
|
||||
""" Given an optimizer and params, iterate until convergence.
|
||||
After each iteration, hook(optimizer) is called.
|
||||
After the function, use values and errors to get the result.
|
||||
Arguments:
|
||||
optimizer {NonlinearOptimizer} -- Nonlinear optimizer
|
||||
params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
|
||||
hook -- hook function to record the error
|
||||
"""
|
||||
def check_convergence(optimizer, current_error, new_error):
|
||||
return (optimizer.iterations() >= params.getMaxIterations()) or (
|
||||
gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
|
||||
current_error, new_error)) or (
|
||||
isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
|
||||
optimize(optimizer, check_convergence, hook)
|
||||
return optimizer.values()
|
||||
|
|
@ -1,401 +0,0 @@
|
|||
"""Various plotting utlities."""
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib import patches
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def set_axes_equal(fignum):
|
||||
"""
|
||||
Make axes of 3D plot have equal scale so that spheres appear as spheres,
|
||||
cubes as cubes, etc.. This is one possible solution to Matplotlib's
|
||||
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
|
||||
|
||||
Args:
|
||||
fignum (int): An integer representing the figure number for Matplotlib.
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
ax = fig.gca(projection='3d')
|
||||
|
||||
limits = np.array([
|
||||
ax.get_xlim3d(),
|
||||
ax.get_ylim3d(),
|
||||
ax.get_zlim3d(),
|
||||
])
|
||||
|
||||
origin = np.mean(limits, axis=1)
|
||||
radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
|
||||
|
||||
ax.set_xlim3d([origin[0] - radius, origin[0] + radius])
|
||||
ax.set_ylim3d([origin[1] - radius, origin[1] + radius])
|
||||
ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
|
||||
|
||||
|
||||
def ellipsoid(xc, yc, zc, rx, ry, rz, n):
|
||||
"""
|
||||
Numpy equivalent of Matlab's ellipsoid function.
|
||||
|
||||
Args:
|
||||
xc (double): Center of ellipsoid in X-axis.
|
||||
yc (double): Center of ellipsoid in Y-axis.
|
||||
zc (double): Center of ellipsoid in Z-axis.
|
||||
rx (double): Radius of ellipsoid in X-axis.
|
||||
ry (double): Radius of ellipsoid in Y-axis.
|
||||
rz (double): Radius of ellipsoid in Z-axis.
|
||||
n (int): The granularity of the ellipsoid plotted.
|
||||
|
||||
Returns:
|
||||
tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
|
||||
"""
|
||||
u = np.linspace(0, 2*np.pi, n+1)
|
||||
v = np.linspace(0, np.pi, n+1)
|
||||
x = -rx * np.outer(np.cos(u), np.sin(v)).T
|
||||
y = -ry * np.outer(np.sin(u), np.sin(v)).T
|
||||
z = -rz * np.outer(np.ones_like(u), np.cos(v)).T
|
||||
|
||||
return x, y, z
|
||||
|
||||
|
||||
def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
|
||||
"""
|
||||
Plots a Gaussian as an uncertainty ellipse
|
||||
|
||||
Based on Maybeck Vol 1, page 366
|
||||
k=2.296 corresponds to 1 std, 68.26% of all probability
|
||||
k=11.82 corresponds to 3 std, 99.74% of all probability
|
||||
|
||||
Args:
|
||||
axes (matplotlib.axes.Axes): Matplotlib axes.
|
||||
origin (gtsam.Point3): The origin in the world frame.
|
||||
P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
|
||||
scale (float): Scaling factor of the radii of the covariance ellipse.
|
||||
n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
|
||||
alpha (float): Transparency value for the plotted surface in the range [0, 1].
|
||||
"""
|
||||
k = 11.82
|
||||
U, S, _ = np.linalg.svd(P)
|
||||
|
||||
radii = k * np.sqrt(S)
|
||||
radii = radii * scale
|
||||
rx, ry, rz = radii
|
||||
|
||||
# generate data for "unrotated" ellipsoid
|
||||
xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n)
|
||||
|
||||
# rotate data with orientation matrix U and center c
|
||||
data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
|
||||
np.kron(U[:, 2:3], zc)
|
||||
n = data.shape[1]
|
||||
x = data[0:n, :] + origin[0]
|
||||
y = data[n:2*n, :] + origin[1]
|
||||
z = data[2*n:, :] + origin[2]
|
||||
|
||||
axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
|
||||
|
||||
|
||||
def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
|
||||
"""
|
||||
Plot a 2D pose on given axis `axes` with given `axis_length`.
|
||||
|
||||
Args:
|
||||
axes (matplotlib.axes.Axes): Matplotlib axes.
|
||||
pose (gtsam.Pose2): The pose to be plotted.
|
||||
axis_length (float): The length of the camera axes.
|
||||
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
"""
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
t = pose.translation()
|
||||
origin = t
|
||||
|
||||
# draw the camera axes
|
||||
x_axis = origin + gRp[:, 0] * axis_length
|
||||
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], 'r-')
|
||||
|
||||
y_axis = origin + gRp[:, 1] * axis_length
|
||||
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], 'g-')
|
||||
|
||||
if covariance is not None:
|
||||
pPp = covariance[0:2, 0:2]
|
||||
gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
|
||||
|
||||
w, v = np.linalg.eig(gPp)
|
||||
|
||||
# k = 2.296
|
||||
k = 5.0
|
||||
|
||||
angle = np.arctan2(v[1, 0], v[0, 0])
|
||||
e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k),
|
||||
np.rad2deg(angle), fill=False)
|
||||
axes.add_patch(e1)
|
||||
|
||||
|
||||
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
|
||||
axis_labels=('X axis', 'Y axis', 'Z axis')):
|
||||
"""
|
||||
Plot a 2D pose on given figure with given `axis_length`.
|
||||
|
||||
Args:
|
||||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
pose (gtsam.Pose2): The pose to be plotted.
|
||||
axis_length (float): The length of the camera axes.
|
||||
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
"""
|
||||
# get figure object
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
plot_pose2_on_axes(axes, pose, axis_length=axis_length,
|
||||
covariance=covariance)
|
||||
|
||||
axes.set_xlabel(axis_labels[0])
|
||||
axes.set_ylabel(axis_labels[1])
|
||||
|
||||
return fig
|
||||
|
||||
|
||||
def plot_point3_on_axes(axes, point, linespec, P=None):
|
||||
"""
|
||||
Plot a 3D point on given axis `axes` with given `linespec`.
|
||||
|
||||
Args:
|
||||
axes (matplotlib.axes.Axes): Matplotlib axes.
|
||||
point (gtsam.Point3): The point to be plotted.
|
||||
linespec (string): String representing formatting options for Matplotlib.
|
||||
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
"""
|
||||
axes.plot([point[0]], [point[1]], [point[2]], linespec)
|
||||
if P is not None:
|
||||
plot_covariance_ellipse_3d(axes, point, P)
|
||||
|
||||
|
||||
def plot_point3(fignum, point, linespec, P=None,
|
||||
axis_labels=('X axis', 'Y axis', 'Z axis')):
|
||||
"""
|
||||
Plot a 3D point on given figure with given `linespec`.
|
||||
|
||||
Args:
|
||||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
point (gtsam.Point3): The point to be plotted.
|
||||
linespec (string): String representing formatting options for Matplotlib.
|
||||
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
|
||||
Returns:
|
||||
fig: The matplotlib figure.
|
||||
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plot_point3_on_axes(axes, point, linespec, P)
|
||||
|
||||
axes.set_xlabel(axis_labels[0])
|
||||
axes.set_ylabel(axis_labels[1])
|
||||
axes.set_zlabel(axis_labels[2])
|
||||
|
||||
return fig
|
||||
|
||||
|
||||
def plot_3d_points(fignum, values, linespec="g*", marginals=None,
|
||||
title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
|
||||
"""
|
||||
Plots the Point3s in `values`, with optional covariances.
|
||||
Finds all the Point3 objects in the given Values object and plots them.
|
||||
If a Marginals object is given, this function will also plot marginal
|
||||
covariance ellipses for each point.
|
||||
|
||||
Args:
|
||||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
values (gtsam.Values): Values dictionary consisting of points to be plotted.
|
||||
linespec (string): String representing formatting options for Matplotlib.
|
||||
marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
title (string): The title of the plot.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
"""
|
||||
|
||||
keys = values.keys()
|
||||
|
||||
# Plot points and covariance matrices
|
||||
for key in keys:
|
||||
try:
|
||||
point = values.atPoint3(key)
|
||||
if marginals is not None:
|
||||
covariance = marginals.marginalCovariance(key)
|
||||
else:
|
||||
covariance = None
|
||||
|
||||
fig = plot_point3(fignum, point, linespec, covariance,
|
||||
axis_labels=axis_labels)
|
||||
|
||||
except RuntimeError:
|
||||
continue
|
||||
# I guess it's not a Point3
|
||||
|
||||
fig.suptitle(title)
|
||||
fig.canvas.set_window_title(title.lower())
|
||||
|
||||
|
||||
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
|
||||
"""
|
||||
Plot a 3D pose on given axis `axes` with given `axis_length`.
|
||||
|
||||
Args:
|
||||
axes (matplotlib.axes.Axes): Matplotlib axes.
|
||||
point (gtsam.Point3): The point to be plotted.
|
||||
linespec (string): String representing formatting options for Matplotlib.
|
||||
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
"""
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
origin = pose.translation()
|
||||
|
||||
# draw the camera axes
|
||||
x_axis = origin + gRp[:, 0] * axis_length
|
||||
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')
|
||||
|
||||
y_axis = origin + gRp[:, 1] * axis_length
|
||||
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')
|
||||
|
||||
z_axis = origin + gRp[:, 2] * axis_length
|
||||
line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
|
||||
|
||||
# plot the covariance
|
||||
if P is not None:
|
||||
# covariance matrix in pose coordinate frame
|
||||
pPp = P[3:6, 3:6]
|
||||
# convert the covariance matrix to global coordinate frame
|
||||
gPp = gRp @ pPp @ gRp.T
|
||||
plot_covariance_ellipse_3d(axes, origin, gPp)
|
||||
|
||||
|
||||
def plot_pose3(fignum, pose, axis_length=0.1, P=None,
|
||||
axis_labels=('X axis', 'Y axis', 'Z axis')):
|
||||
"""
|
||||
Plot a 3D pose on given figure with given `axis_length`.
|
||||
|
||||
Args:
|
||||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
pose (gtsam.Pose3): 3D pose to be plotted.
|
||||
linespec (string): String representing formatting options for Matplotlib.
|
||||
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
|
||||
Returns:
|
||||
fig: The matplotlib figure.
|
||||
"""
|
||||
# get figure object
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plot_pose3_on_axes(axes, pose, P=P,
|
||||
axis_length=axis_length)
|
||||
|
||||
axes.set_xlabel(axis_labels[0])
|
||||
axes.set_ylabel(axis_labels[1])
|
||||
axes.set_zlabel(axis_labels[2])
|
||||
|
||||
return fig
|
||||
|
||||
|
||||
def plot_trajectory(fignum, values, scale=1, marginals=None,
|
||||
title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
|
||||
"""
|
||||
Plot a complete 3D trajectory using poses in `values`.
|
||||
|
||||
Args:
|
||||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
values (gtsam.Values): Values dict containing the poses.
|
||||
scale (float): Value to scale the poses by.
|
||||
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
|
||||
Used to plot uncertainty bounds.
|
||||
title (string): The title of the plot.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
"""
|
||||
pose3Values = gtsam.utilities.allPose3s(values)
|
||||
keys = gtsam.KeyVector(pose3Values.keys())
|
||||
lastKey = None
|
||||
|
||||
for key in keys:
|
||||
try:
|
||||
pose = pose3Values.atPose3(key)
|
||||
except:
|
||||
print("Warning: no Pose3 at key: {0}".format(key))
|
||||
|
||||
if lastKey is not None:
|
||||
try:
|
||||
lastPose = pose3Values.atPose3(lastKey)
|
||||
except:
|
||||
print("Warning: no Pose3 at key: {0}".format(lastKey))
|
||||
pass
|
||||
|
||||
if marginals:
|
||||
covariance = marginals.marginalCovariance(lastKey)
|
||||
else:
|
||||
covariance = None
|
||||
|
||||
fig = plot_pose3(fignum, lastPose, P=covariance,
|
||||
axis_length=scale, axis_labels=axis_labels)
|
||||
|
||||
lastKey = key
|
||||
|
||||
# Draw final pose
|
||||
if lastKey is not None:
|
||||
try:
|
||||
lastPose = pose3Values.atPose3(lastKey)
|
||||
if marginals:
|
||||
covariance = marginals.marginalCovariance(lastKey)
|
||||
else:
|
||||
covariance = None
|
||||
|
||||
fig = plot_pose3(fignum, lastPose, P=covariance,
|
||||
axis_length=scale, axis_labels=axis_labels)
|
||||
|
||||
except:
|
||||
pass
|
||||
|
||||
fig.suptitle(title)
|
||||
fig.canvas.set_window_title(title.lower())
|
||||
|
||||
|
||||
def plot_incremental_trajectory(fignum, values, start=0,
|
||||
scale=1, marginals=None,
|
||||
time_interval=0.0):
|
||||
"""
|
||||
Incrementally plot a complete 3D trajectory using poses in `values`.
|
||||
|
||||
Args:
|
||||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
values (gtsam.Values): Values dict containing the poses.
|
||||
start (int): Starting index to start plotting from.
|
||||
scale (float): Value to scale the poses by.
|
||||
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
|
||||
Used to plot uncertainty bounds.
|
||||
time_interval (float): Time in seconds to pause between each rendering.
|
||||
Used to create animation effect.
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
|
||||
pose3Values = gtsam.utilities.allPose3s(values)
|
||||
keys = gtsam.KeyVector(pose3Values.keys())
|
||||
|
||||
for key in keys[start:]:
|
||||
if values.exists(key):
|
||||
pose_i = values.atPose3(key)
|
||||
plot_pose3(fignum, pose_i, scale)
|
||||
|
||||
# Update the plot space to encompass all plotted points
|
||||
axes.autoscale()
|
||||
|
||||
# Set the 3 axes equal
|
||||
set_axes_equal(fignum)
|
||||
|
||||
# Pause for a fixed amount of seconds
|
||||
plt.pause(time_interval)
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
TestCase class with GTSAM assert utils.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
import unittest
|
||||
|
||||
|
||||
class GtsamTestCase(unittest.TestCase):
|
||||
"""TestCase class with GTSAM assert utils."""
|
||||
|
||||
def gtsamAssertEquals(self, actual, expected, tol=1e-9):
|
||||
""" AssertEqual function that prints out actual and expected if not equal.
|
||||
Usage:
|
||||
self.gtsamAssertEqual(actual,expected)
|
||||
Keyword Arguments:
|
||||
tol {float} -- tolerance passed to 'equals', default 1e-9
|
||||
"""
|
||||
import numpy
|
||||
if isinstance(expected, numpy.ndarray):
|
||||
equal = numpy.allclose(actual, expected, atol=tol)
|
||||
else:
|
||||
equal = actual.equals(expected, tol)
|
||||
if not equal:
|
||||
raise self.failureException(
|
||||
"Values are not equal:\n{}!={}".format(actual, expected))
|
||||
|
|
@ -1,119 +0,0 @@
|
|||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
from math import pi
|
||||
import gtsam
|
||||
from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
|
||||
|
||||
|
||||
class Options:
|
||||
"""
|
||||
Options to generate test scenario
|
||||
"""
|
||||
|
||||
def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
|
||||
"""
|
||||
Options to generate test scenario
|
||||
@param triangle: generate a triangle scene with 3 points if True, otherwise
|
||||
a cube with 8 points
|
||||
@param nrCameras: number of cameras to generate
|
||||
@param K: camera calibration object
|
||||
"""
|
||||
self.triangle = triangle
|
||||
self.nrCameras = nrCameras
|
||||
|
||||
|
||||
class GroundTruth:
|
||||
"""
|
||||
Object holding generated ground-truth data
|
||||
"""
|
||||
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.cameras = [Pose3()] * nrCameras
|
||||
self.points = [Point3(0, 0, 0)] * nrPoints
|
||||
|
||||
def print_(self, s=""):
|
||||
print(s)
|
||||
print("K = ", self.K)
|
||||
print("Cameras: ", len(self.cameras))
|
||||
for camera in self.cameras:
|
||||
print("\t", camera)
|
||||
print("Points: ", len(self.points))
|
||||
for point in self.points:
|
||||
print("\t", point)
|
||||
pass
|
||||
|
||||
|
||||
class Data:
|
||||
"""
|
||||
Object holding generated measurement data
|
||||
"""
|
||||
|
||||
class NoiseModels:
|
||||
pass
|
||||
|
||||
def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
|
||||
self.K = K
|
||||
self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
|
||||
self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
|
||||
self.odometry = [Pose3()] * nrCameras
|
||||
|
||||
# Set Noise parameters
|
||||
self.noiseModels = Data.NoiseModels()
|
||||
self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
|
||||
# noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
# np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
|
||||
self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
|
||||
self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||
self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
|
||||
|
||||
|
||||
def generate_data(options):
|
||||
""" Generate ground-truth and measurement data. """
|
||||
|
||||
K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
|
||||
nrPoints = 3 if options.triangle else 8
|
||||
|
||||
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
|
||||
data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
|
||||
|
||||
# Generate simulated data
|
||||
if options.triangle: # Create a triangle target, just 3 points on a plane
|
||||
r = 10
|
||||
for j in range(len(truth.points)):
|
||||
theta = j * 2 * pi / nrPoints
|
||||
truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
|
||||
else: # 3D landmarks as vertices of a cube
|
||||
truth.points = [
|
||||
Point3(10, 10, 10), Point3(-10, 10, 10),
|
||||
Point3(-10, -10, 10), Point3(10, -10, 10),
|
||||
Point3(10, 10, -10), Point3(-10, 10, -10),
|
||||
Point3(-10, -10, -10), Point3(10, -10, -10)
|
||||
]
|
||||
|
||||
# Create camera cameras on a circle around the triangle
|
||||
height = 10
|
||||
r = 40
|
||||
for i in range(options.nrCameras):
|
||||
theta = i * 2 * pi / options.nrCameras
|
||||
t = Point3(r * math.cos(theta), r * math.sin(theta), height)
|
||||
truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
|
||||
Point3(0, 0, 0),
|
||||
Point3(0, 0, 1),
|
||||
truth.K)
|
||||
# Create measurements
|
||||
for j in range(nrPoints):
|
||||
# All landmarks seen in every frame
|
||||
data.Z[i][j] = truth.cameras[i].project(truth.points[j])
|
||||
data.J[i][j] = j
|
||||
|
||||
# Calculate odometry between cameras
|
||||
for i in range(1, options.nrCameras):
|
||||
data.odometry[i] = truth.cameras[i - 1].pose().between(
|
||||
truth.cameras[i].pose())
|
||||
|
||||
return data, truth
|
||||
|
|
@ -1,131 +0,0 @@
|
|||
import gtsam
|
||||
from gtsam import symbol
|
||||
|
||||
|
||||
class Options:
|
||||
""" Options for visual isam example. """
|
||||
|
||||
def __init__(self):
|
||||
self.hardConstraint = False
|
||||
self.pointPriors = False
|
||||
self.batchInitialization = True
|
||||
self.reorderInterval = 10
|
||||
self.alwaysRelinearize = False
|
||||
|
||||
|
||||
def initialize(data, truth, options):
|
||||
# Initialize iSAM
|
||||
params = gtsam.ISAM2Params()
|
||||
if options.alwaysRelinearize:
|
||||
params.setRelinearizeSkip(1)
|
||||
isam = gtsam.ISAM2(params=params)
|
||||
|
||||
# Add constraints/priors
|
||||
# TODO: should not be from ground truth!
|
||||
newFactors = gtsam.NonlinearFactorGraph()
|
||||
initialEstimates = gtsam.Values()
|
||||
for i in range(2):
|
||||
ii = symbol('x', i)
|
||||
if i == 0:
|
||||
if options.hardConstraint: # add hard constraint
|
||||
newFactors.add(
|
||||
gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
|
||||
else:
|
||||
newFactors.add(
|
||||
gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
|
||||
data.noiseModels.posePrior))
|
||||
initialEstimates.insert(ii, truth.cameras[i].pose())
|
||||
|
||||
nextPoseIndex = 2
|
||||
|
||||
# Add visual measurement factors from two first poses and initialize
|
||||
# observed landmarks
|
||||
for i in range(2):
|
||||
ii = symbol('x', i)
|
||||
for k in range(len(data.Z[i])):
|
||||
j = data.J[i][k]
|
||||
jj = symbol('l', j)
|
||||
newFactors.add(
|
||||
gtsam.GenericProjectionFactorCal3_S2(data.Z[i][
|
||||
k], data.noiseModels.measurement, ii, jj, data.K))
|
||||
# TODO: initial estimates should not be from ground truth!
|
||||
if not initialEstimates.exists(jj):
|
||||
initialEstimates.insert(jj, truth.points[j])
|
||||
if options.pointPriors: # add point priors
|
||||
newFactors.add(
|
||||
gtsam.PriorFactorPoint3(jj, truth.points[j],
|
||||
data.noiseModels.pointPrior))
|
||||
|
||||
# Add odometry between frames 0 and 1
|
||||
newFactors.add(
|
||||
gtsam.BetweenFactorPose3(
|
||||
symbol('x', 0),
|
||||
symbol('x', 1), data.odometry[1], data.noiseModels.odometry))
|
||||
|
||||
# Update ISAM
|
||||
if options.batchInitialization: # Do a full optimize for first two poses
|
||||
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors,
|
||||
initialEstimates)
|
||||
fullyOptimized = batchOptimizer.optimize()
|
||||
isam.update(newFactors, fullyOptimized)
|
||||
else:
|
||||
isam.update(newFactors, initialEstimates)
|
||||
|
||||
# figure(1)tic
|
||||
# t=toc plot(frame_i,t,'r.') tic
|
||||
result = isam.calculateEstimate()
|
||||
# t=toc plot(frame_i,t,'g.')
|
||||
|
||||
return isam, result, nextPoseIndex
|
||||
|
||||
|
||||
def step(data, isam, result, truth, currPoseIndex):
|
||||
'''
|
||||
Do one step isam update
|
||||
@param[in] data: measurement data (odometry and visual measurements and their noiseModels)
|
||||
@param[in/out] isam: current isam object, will be updated
|
||||
@param[in/out] result: current result object, will be updated
|
||||
@param[in] truth: ground truth data, used to initialize new variables
|
||||
@param[currPoseIndex]: index of the current pose
|
||||
'''
|
||||
# iSAM expects us to give it a new set of factors
|
||||
# along with initial estimates for any new variables introduced.
|
||||
newFactors = gtsam.NonlinearFactorGraph()
|
||||
initialEstimates = gtsam.Values()
|
||||
|
||||
# Add odometry
|
||||
prevPoseIndex = currPoseIndex - 1
|
||||
odometry = data.odometry[prevPoseIndex]
|
||||
newFactors.add(
|
||||
gtsam.BetweenFactorPose3(
|
||||
symbol('x', prevPoseIndex),
|
||||
symbol('x', currPoseIndex), odometry,
|
||||
data.noiseModels.odometry))
|
||||
|
||||
# Add visual measurement factors and initializations as necessary
|
||||
for k in range(len(data.Z[currPoseIndex])):
|
||||
zij = data.Z[currPoseIndex][k]
|
||||
j = data.J[currPoseIndex][k]
|
||||
jj = symbol('l', j)
|
||||
newFactors.add(
|
||||
gtsam.GenericProjectionFactorCal3_S2(
|
||||
zij, data.noiseModels.measurement,
|
||||
symbol('x', currPoseIndex), jj, data.K))
|
||||
# TODO: initialize with something other than truth
|
||||
if not result.exists(jj) and not initialEstimates.exists(jj):
|
||||
lmInit = truth.points[j]
|
||||
initialEstimates.insert(jj, lmInit)
|
||||
|
||||
# Initial estimates for the new pose.
|
||||
prevPose = result.atPose3(symbol('x', prevPoseIndex))
|
||||
initialEstimates.insert(
|
||||
symbol('x', currPoseIndex), prevPose.compose(odometry))
|
||||
|
||||
# Update ISAM
|
||||
# figure(1)tic
|
||||
isam.update(newFactors, initialEstimates)
|
||||
# t=toc plot(frame_i,t,'r.') tic
|
||||
newResult = isam.calculateEstimate()
|
||||
# t=toc plot(frame_i,t,'g.')
|
||||
|
||||
return isam, newResult
|
||||
|
|
@ -1 +0,0 @@
|
|||
from .gtsam_unstable import *
|
||||
|
|
@ -1,92 +0,0 @@
|
|||
"""
|
||||
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 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((X1, 0.0))
|
||||
|
||||
delta_time = 0.25
|
||||
time = 0.25
|
||||
|
||||
while time <= 3.0:
|
||||
previous_key = int(1000 * (time - delta_time))
|
||||
current_key = int(1000 * time)
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps.insert((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")
|
||||
|
|
@ -1,128 +0,0 @@
|
|||
"""
|
||||
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)
|
||||
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")
|
||||
|
|
@ -1,32 +0,0 @@
|
|||
{include_boost}
|
||||
|
||||
#include <pybind11/eigen.h>
|
||||
#include <pybind11/stl_bind.h>
|
||||
#include <pybind11/pybind11.h>
|
||||
#include "gtsam/base/serialization.h"
|
||||
#include "gtsam/nonlinear/utilities.h" // for RedirectCout.
|
||||
|
||||
{includes}
|
||||
#include <boost/serialization/export.hpp>
|
||||
|
||||
{boost_class_export}
|
||||
|
||||
{hoder_type}
|
||||
|
||||
#include "python/gtsam_unstable/preamble.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace py = pybind11;
|
||||
|
||||
PYBIND11_MODULE({module_name}, m_) {{
|
||||
m_.doc() = "pybind11 wrapper of {module_name}";
|
||||
|
||||
py::module::import("gtsam");
|
||||
|
||||
{wrapped_namespace}
|
||||
|
||||
#include "python/gtsam_unstable/specializations.h"
|
||||
|
||||
}}
|
||||
|
||||
|
|
@ -1,128 +0,0 @@
|
|||
"""
|
||||
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
|
||||
|
||||
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((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 = int(1000 * (time - delta_time))
|
||||
current_key = int(1000 * time)
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps.insert((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()
|
||||
|
|
@ -1,2 +0,0 @@
|
|||
numpy>=1.11.0
|
||||
pyparsing>=2.4.2
|
||||
|
|
@ -1,49 +0,0 @@
|
|||
import os
|
||||
import sys
|
||||
|
||||
try:
|
||||
from setuptools import setup, find_packages
|
||||
except ImportError:
|
||||
from distutils.core import setup, find_packages
|
||||
|
||||
packages = find_packages(where=".")
|
||||
print("PACKAGES: ", packages)
|
||||
package_data = {
|
||||
'': [
|
||||
'./*.so',
|
||||
'./*.dll',
|
||||
]
|
||||
}
|
||||
|
||||
# Cleaner to read in the contents rather than copy them over.
|
||||
readme_contents = open("${PROJECT_SOURCE_DIR}/README.md").read()
|
||||
|
||||
setup(
|
||||
name='gtsam',
|
||||
description='Georgia Tech Smoothing And Mapping library',
|
||||
url='https://gtsam.org/',
|
||||
version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/
|
||||
author='Frank Dellaert et. al.',
|
||||
author_email='frank.dellaert@gtsam.org',
|
||||
license='Simplified BSD license',
|
||||
keywords='slam sam robotics localization mapping optimization',
|
||||
long_description=readme_contents,
|
||||
# https://pypi.org/pypi?%3Aaction=list_classifiers
|
||||
classifiers=[
|
||||
'Development Status :: 5 - Production/Stable',
|
||||
'Intended Audience :: Education',
|
||||
'Intended Audience :: Developers',
|
||||
'Intended Audience :: Science/Research',
|
||||
'Operating System :: MacOS',
|
||||
'Operating System :: Microsoft :: Windows',
|
||||
'Operating System :: POSIX',
|
||||
'License :: OSI Approved :: BSD License',
|
||||
'Programming Language :: Python :: 2',
|
||||
'Programming Language :: Python :: 3',
|
||||
],
|
||||
packages=packages,
|
||||
package_data=package_data,
|
||||
test_suite="gtsam.tests",
|
||||
install_requires=["numpy"],
|
||||
zip_safe=False,
|
||||
)
|
||||
Loading…
Reference in New Issue