12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Sync with new_wrapper develop branch

release/4.3a0
Fan Jiang 2020-07-31 12:39:04 -04:00
parent fda79057e4
commit 1cabd2000f
6 changed files with 37 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
python/gtsam/__init__.py
View File

@ -1,16 +1,21 @@
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

2
python/gtsam/examples/ImuFactorISAM2Example.py
View File

@ -1,6 +1,6 @@
"""
iSAM2 example with ImuFactor.
Author: Frank Dellaert, Varun Agrawal
Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal
"""
# pylint: disable=invalid-name, E1101

12
python/gtsam/examples/README.md
View File

@ -5,8 +5,8 @@
| CameraResectioning | |
| CreateSFMExampleData | |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through Python |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through Python |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python |
| ImuFactorExample2 | X |
| ImuFactorsExample | |
| ISAM2Example_SmartFactor | |
@ -20,7 +20,7 @@
| Pose2SLAMExample_g2o | X |
| Pose2SLAMExample_graph | |
| Pose2SLAMExample_graphviz | |
| Pose2SLAMExample_lago | lago not exposed through Python |
| Pose2SLAMExample_lago | lago not yet exposed through Python |
| Pose2SLAMStressTest | |
| Pose2SLAMwSPCG | |
| Pose3SLAMExample_changeKeys | |
@ -42,11 +42,11 @@
| StereoVOExample | |
| StereoVOExample_large | |
| TimeTBB | |
| UGM_chain | discrete functionality not exposed |
| UGM_small | discrete functionality not exposed |
| 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
- SFMData

17
python/gtsam/examples/VisualISAMExample.py
View File

@ -19,12 +19,7 @@ from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values, Point3)
def symbol(name: str, index: int) -> int:
""" helper for creating a symbol without explicitly casting 'name' from str to int """
return gtsam.symbol(name, index)
from gtsam.symbol_shorthand import X, L
def main():
"""
@ -60,7 +55,7 @@ def main():
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, camera_noise, symbol('x', i), symbol('l', j), K)
measurement, camera_noise, X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
@ -69,7 +64,7 @@ def main():
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(symbol('x', i), initial_xi)
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
@ -79,12 +74,12 @@ def main():
# 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(symbol('x', 0), poses[0], pose_noise)
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(symbol('l', 0), points[0], point_noise)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
@ -92,7 +87,7 @@ def main():
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j] + noise
initial_estimate.insert(symbol('l', j), initial_lj)
initial_estimate.insert(L(j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)

20
python/gtsam/tests/test_SFMExample.py
View File

@ -17,7 +17,7 @@ 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):
@ -41,23 +41,23 @@ class TestSFMExample(GtsamTestCase):
j = data.J[i][k]
graph.add(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], measurementNoise,
symbol('x', i), symbol('p', j), data.K))
X(i), P(j), data.K))
posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(symbol('x', 0),
graph.add(gtsam.PriorFactorPose3(X(0),
truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(symbol('p', 0),
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(symbol('x', i), pose_i)
initialEstimate.insert(X(i), pose_i)
for j in range(len(truth.points)):
point_j = truth.points[j]
initialEstimate.insert(symbol('p', j), point_j)
initialEstimate.insert(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
@ -67,16 +67,16 @@ class TestSFMExample(GtsamTestCase):
# Marginalization
marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(symbol('p', 0))
marginals.marginalCovariance(symbol('x', 0))
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(symbol('x', i))
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(symbol('p', j))
point_j = result.atPoint3(P(j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
if __name__ == "__main__":

23
python/gtsam/utils/plot.py
View File

@ -109,7 +109,7 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
origin = np.array(t)
origin = t
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
@ -221,9 +221,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
keys = values.keys()
# Plot points and covariance matrices
for i in range(len(keys)):
for key in keys:
try:
key = keys[i]
point = values.atPoint3(key)
if marginals is not None:
covariance = marginals.marginalCovariance(key)
@ -321,17 +320,15 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
"""
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastIndex = None
lastKey = None
for i in range(len(keys)):
key = keys[i]
for key in keys:
try:
pose = pose3Values.atPose3(key)
except:
print("Warning: no Pose3 at key: {0}".format(key))
if lastIndex is not None:
lastKey = keys[lastIndex]
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
except:
@ -346,11 +343,10 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
lastIndex = i
lastKey = key
# Draw final pose
if lastIndex is not None:
lastKey = keys[lastIndex]
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
@ -390,10 +386,9 @@ def plot_incremental_trajectory(fignum, values, start=0,
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
for i in range(start, len(keys)):
key = keys[i]
for key in keys[start:]:
if values.exists(key):
pose_i = values.atPose3(keys[i])
pose_i = values.atPose3(key)
plot_pose3(fignum, pose_i, scale)
# Update the plot space to encompass all plotted points