2 GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
9 A visualSLAM example for the structure-from-motion problem on a simulated dataset
10 This version uses iSAM to solve the problem incrementally
16 from gtsam
import (Cal3_S2, GenericProjectionFactorCal3_S2,
17 NonlinearFactorGraph, NonlinearISAM, Pose3,
18 PriorFactorPoint3, PriorFactorPose3, Rot3,
19 PinholeCameraCal3_S2, Values, Point3)
24 A structure-from-motion example with landmarks
25 - The landmarks form a 10 meter cube
26 - The robot rotates around the landmarks, always facing towards the cube
30 K =
Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
37 points = SFMdata.createPoints()
39 poses = SFMdata.createPoses()
47 initial_estimate =
Values()
50 for i, pose
in enumerate(poses):
53 for j, point
in enumerate(points):
54 measurement = camera.project(point)
56 measurement, camera_noise,
X(i),
L(j), K)
57 graph.push_back(factor)
60 noise =
Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
61 t=
Point3(0.05, -0.10, 0.20))
62 initial_xi = pose.compose(noise)
65 initial_estimate.insert(
X(i), initial_xi)
74 np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
76 graph.push_back(factor)
81 graph.push_back(factor)
84 noise = np.array([-0.25, 0.20, 0.15])
85 for j, point
in enumerate(points):
87 initial_lj = points[j] + noise
88 initial_estimate.insert(
L(j), initial_lj)
91 isam.update(graph, initial_estimate)
92 current_estimate = isam.estimate()
95 current_estimate.print(
'Current estimate: ')
99 initial_estimate.clear()
102 if __name__ ==
'__main__':