test_SFMExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 SFM unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 import unittest
12 
13 import numpy as np
14 
15 import gtsam
16 import gtsam.utils.visual_data_generator as generator
17 from gtsam import symbol
18 from gtsam.noiseModel import Isotropic, Diagonal
19 from gtsam.utils.test_case import GtsamTestCase
20 from gtsam.symbol_shorthand import X, P
21 
23 
24  def test_SFMExample(self):
25  options = generator.Options()
26  options.triangle = False
27  options.nrCameras = 10
28 
29  [data, truth] = generator.generate_data(options)
30 
31  measurementNoiseSigma = 1.0
32  pointNoiseSigma = 0.1
33  poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
34 
36 
37  # Add factors for all measurements
38  measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma)
39  for i in range(len(data.Z)):
40  for k in range(len(data.Z[i])):
41  j = data.J[i][k]
42  graph.add(gtsam.GenericProjectionFactorCal3_S2(
43  data.Z[i][k], measurementNoise,
44  X(i), P(j), data.K))
45 
46  posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
47  graph.add(gtsam.PriorFactorPose3(X(0),
48  truth.cameras[0].pose(), posePriorNoise))
49  pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
50  graph.add(gtsam.PriorFactorPoint3(P(0),
51  truth.points[0], pointPriorNoise))
52 
53  # Initial estimate
54  initialEstimate = gtsam.Values()
55  for i in range(len(truth.cameras)):
56  pose_i = truth.cameras[i].pose()
57  initialEstimate.insert(X(i), pose_i)
58  for j in range(len(truth.points)):
59  point_j = truth.points[j]
60  initialEstimate.insert(P(j), point_j)
61 
62  # Optimization
63  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
64  for i in range(5):
65  optimizer.iterate()
66  result = optimizer.values()
67 
68  # Marginalization
69  marginals = gtsam.Marginals(graph, result)
70  marginals.marginalCovariance(P(0))
71  marginals.marginalCovariance(X(0))
72 
73  # Check optimized results, should be equal to ground truth
74  for i in range(len(truth.cameras)):
75  pose_i = result.atPose3(X(i))
76  self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
77 
78  for j in range(len(truth.points)):
79  point_j = result.atPoint3(P(j))
80  self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
81 
82 if __name__ == "__main__":
83  unittest.main()
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Double_ range(const Point2_ &p, const Point2_ &q)
#define X
Definition: icosphere.cpp:20
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2244
All noise models live in the noiseModel namespace.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:46