test_StereoVOExample.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 Stereo VO 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 from gtsam import symbol
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 
21 
23 
29 
30 
31  x1 = symbol('x',1)
32  x2 = symbol('x',2)
33  l1 = symbol('l',1)
34  l2 = symbol('l',2)
35  l3 = symbol('l',3)
36 
37 
39 
40 
41  first_pose = gtsam.Pose3()
42  graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
43 
44 
46  K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
47  stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
48 
49 
51  graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
52  graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
53  graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
54 
55  #pose 2
56  graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
57  graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K))
58  graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
59 
60 
61  initialEstimate = gtsam.Values()
62  initialEstimate.insert(x1, first_pose)
63  # noisy estimate for pose 2
64  initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1)))
65  expected_l1 = gtsam.Point3( 1, 1, 5)
66  initialEstimate.insert(l1, expected_l1)
67  initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
68  initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5))
69 
70 
71  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
72  result = optimizer.optimize()
73 
74 
75  pose_x1 = result.atPose3(x1)
76  self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
77 
78  point_l1 = result.atPoint3(l1)
79  self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
80 
81 if __name__ == "__main__":
82  unittest.main()
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
test_StereoVOExample.TestStereoVOExample
Definition: test_StereoVOExample.py:20
gtsam::Values
Definition: Values.h:65
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
test_StereoVOExample.TestStereoVOExample.test_StereoVOExample
def test_StereoVOExample(self)
Definition: test_StereoVOExample.py:22
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:31