test_initialize_pose3.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 Unit tests for 3D SLAM initialization, using rotation relaxation.
9 Author: Luca Carlone and Frank Dellaert (Python)
10 """
11 # pylint: disable=invalid-name, E1101, E0611
12 import unittest
13 
14 import numpy as np
15 
16 import gtsam
17 from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
18 from gtsam.utils.test_case import GtsamTestCase
19 
20 x0, x1, x2, x3 = 0, 1, 2, 3
21 
22 
24 
25  def setUp(self):
26 
27  model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
28 
29  # We consider a small graph:
30  # symbolic FG
31  # x2 0 1
32  # / | \ 1 2
33  # / | \ 2 3
34  # x3 | x1 2 0
35  # \ | / 0 3
36  # \ | /
37  # x0
38  #
39  p0 = Point3(0, 0, 0)
40  self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
41  p1 = Point3(1, 2, 0)
42  self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
43  p2 = Point3(0, 2, 0)
44  self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
45  p3 = Point3(-1, 1, 0)
46  self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
47 
48  pose0 = Pose3(self.R0, p0)
49  pose1 = Pose3(self.R1, p1)
50  pose2 = Pose3(self.R2, p2)
51  pose3 = Pose3(self.R3, p3)
52 
54  g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
55  g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
56  g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
57  g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
58  g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
59  g.add(gtsam.PriorFactorPose3(x0, pose0, model))
60  self.graph = g
61 
64 
65  def test_orientations(self):
68 
69  # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
70  self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
71  self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
72  self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6)
73  self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6)
74 
76  g2oFile = gtsam.findExampleDataFile("pose3example-grid")
77  is3D = True
78  inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
79  priorModel = gtsam.noiseModel.Unit.Create(6)
80  inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
81 
82  initial = gtsam.InitializePose3.initialize(inputGraph)
83  # TODO(frank): very loose !!
84  self.gtsamAssertEquals(initial, expectedValues, 0.1)
85 
86 
87 if __name__ == "__main__":
88  unittest.main()
static Values computeOrientationsChordal(const NonlinearFactorGraph &pose3Graph)
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610
static Values initialize(const NonlinearFactorGraph &graph, const Values &givenGuess, bool useGradient=false)
static NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph &graph)
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
Vector3 Point3
Definition: Point3.h:38
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594


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