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

modernized SFM example and added plotting of trajectory and landmarks

release/4.3a0
Varun Agrawal 2020-03-20 12:12:06 -04:00
parent b3a9c7a5ef
commit 81b4765299
3 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cython/gtsam/examples/SFMExample.py
View File

@ -10,13 +10,17 @@ A structure-from-motion problem on a simulated dataset
""" """
from __future__ import print_function from __future__ import print_function
import gtsam import matplotlib.pyplot as plt
import numpy as np import numpy as np
import gtsam
from gtsam.examples import SFMdata from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, NonlinearFactorGraph, GenericProjectionFactorCal3_S2, Marginals,
Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, NonlinearFactorGraph, Point3, Pose3,
Rot3, SimpleCamera, Values) PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
from gtsam.utils import plot
def symbol(name: str, index: int) -> int: def symbol(name: str, index: int) -> int:
@ -94,12 +98,10 @@ def main():
# Intentionally initialize the variables off from the ground truth # Intentionally initialize the variables off from the ground truth
initial_estimate = Values() initial_estimate = Values()
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
r = Rot3.Rodrigues(-0.1, 0.2, 0.25) transformed_pose = pose.retract(0.1*np.random.randn(6,1))
t = Point3(0.05, -0.10, 0.20)
transformed_pose = pose.compose(Pose3(r, t))
initial_estimate.insert(symbol('x', i), transformed_pose) initial_estimate.insert(symbol('x', i), transformed_pose)
for j, point in enumerate(points): for j, point in enumerate(points):
transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15])) transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(symbol('l', j), transformed_point) initial_estimate.insert(symbol('l', j), transformed_point)
initial_estimate.print_('Initial Estimates:\n') initial_estimate.print_('Initial Estimates:\n')
@ -113,6 +115,11 @@ def main():
print('initial error = {}'.format(graph.error(initial_estimate))) print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result))) 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__': if __name__ == '__main__':
main() main()

5
cython/gtsam/examples/SFMdata.py
View File

@ -25,14 +25,15 @@ def createPoints():
def createPoses(K): def createPoses(K):
# Create the set of ground-truth poses # Create the set of ground-truth poses
radius = 30.0 radius = 40.0
height = 10.0
angles = np.linspace(0, 2*np.pi, 8, endpoint=False) angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1) up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0) target = gtsam.Point3(0, 0, 0)
poses = [] poses = []
for theta in angles: for theta in angles:
position = gtsam.Point3(radius*np.cos(theta), position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta), 0.0) radius*np.sin(theta), height)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose()) poses.append(camera.pose())
return poses return poses