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

Update to match cython

release/4.3a0
Fan Jiang 2020-07-27 14:35:28 -04:00
parent 128db80fec
commit 7b4266ed6b
9 changed files with 37 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
python/gtsam_py/python/examples/ImuFactorExample.py
View File

@ -17,10 +17,7 @@ import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand
B = symbol_shorthand.B
V = symbol_shorthand.V
X = symbol_shorthand.X
from gtsam.gtsam.symbol_shorthand import B, V, X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D

5
python/gtsam_py/python/examples/ImuFactorExample2.py
View File

@ -17,10 +17,7 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
from gtsam import symbol_shorthand
B = symbol_shorthand.B
V = symbol_shorthand.V
X = symbol_shorthand.X
from gtsam.gtsam.symbol_shorthand import B, V, X
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611

11
python/gtsam_py/python/examples/PlanarSLAMExample.py
View File

@ -16,6 +16,7 @@ from __future__ import print_function
import numpy as np
import gtsam
from gtsam.gtsam.symbol_shorthand import X, L
# Create noise models
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = gtsam.symbol('x', 1)
X2 = gtsam.symbol('x', 2)
X3 = gtsam.symbol('x', 3)
L1 = gtsam.symbol('l', 4)
L2 = gtsam.symbol('l', 5)
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))

2
python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py
View File

@ -83,7 +83,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.extractPose2(result)
resultPoses = gtsam.utilities.extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()

4
python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py
View File

@ -44,7 +44,7 @@ priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
firstKey = initial.keys()[0]
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
@ -66,7 +66,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.allPose3s(result)
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()

2
python/gtsam_py/python/examples/PreintegrationExample.py
View File

@ -111,7 +111,7 @@ class PreintegrationExample(object):
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, 0.3)
t = actualPose.translation()
self.maxDim = max([max(t), self.maxDim])
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)

14
python/gtsam_py/python/examples/README.md
View File

@ -1,18 +1,12 @@
# THIS FILE IS OUTDATED!
~~These examples are almost identical to the old handwritten python wrapper examples. However, there are just some slight name changes, for example `noiseModel.Diagonal` becomes `noiseModel.Diagonal` etc...~~
~~Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol('b', 0))`~~
# Porting Progress
| C++ Example Name | Ported |
|-------------------------------------------------------|--------|
| CameraResectioning | |
| CreateSFMExampleData | |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython |
| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python |
| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through Python |
| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through Python |
| ImuFactorExample2 | X |
| ImuFactorsExample | |
| ISAM2Example_SmartFactor | |
@ -26,7 +20,7 @@
| Pose2SLAMExample_g2o | X |
| Pose2SLAMExample_graph | |
| Pose2SLAMExample_graphviz | |
| Pose2SLAMExample_lago | lago not exposed through cython |
| Pose2SLAMExample_lago | lago not exposed through Python |
| Pose2SLAMStressTest | |
| Pose2SLAMwSPCG | |
| Pose3SLAMExample_changeKeys | |

32
python/gtsam_py/python/examples/SFMExample.py
View File

@ -10,24 +10,21 @@ 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
import gtsam
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
from gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
from gtsam.utils import plot
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)
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
@ -74,23 +71,23 @@ def main():
# 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(symbol('x', 0), poses[0], pose_noise)
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 = SimpleCamera(pose, K)
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
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(symbol('l', 0), points[0], point_noise)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
@ -99,10 +96,10 @@ def main():
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
initial_estimate.insert(symbol('x', i), transformed_pose)
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(symbol('l', j), transformed_point)
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
@ -121,6 +118,5 @@ def main():
plot.set_axes_equal(1)
plt.show()
if __name__ == '__main__':
main()

14
python/gtsam_py/python/utils/plot.py
View File

@ -221,9 +221,9 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
keys = values.keys()
# Plot points and covariance matrices
for i in range(keys.size()):
for i in range(len(keys)):
try:
key = keys.at(i)
key = keys[i]
point = values.atPoint3(key)
if marginals is not None:
covariance = marginals.marginalCovariance(key)
@ -319,19 +319,19 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
pose3Values = gtsam.utilities_allPose3s(values)
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastIndex = None
for i in range(keys.size()):
key = keys.at(i)
for i in range(len(keys)):
key = keys[i]
try:
pose = pose3Values.atPose3(key)
except:
print("Warning: no Pose3 at key: {0}".format(key))
if lastIndex is not None:
lastKey = keys.at(lastIndex)
lastKey = keys[lastIndex]
try:
lastPose = pose3Values.atPose3(lastKey)
except:
@ -350,7 +350,7 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
# Draw final pose
if lastIndex is not None:
lastKey = keys.at(lastIndex)
lastKey = keys[lastIndex]
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals: