test_PriorFactor.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 PriorFactor 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.utils.test_case import GtsamTestCase
17 
18 
20 
21  def test_PriorFactor(self):
22  values = gtsam.Values()
23 
24  key = 5
25  priorPose3 = gtsam.Pose3()
27  factor = gtsam.PriorFactorPose3(key, priorPose3, model)
28  values.insert(key, priorPose3)
29  self.assertEqual(factor.error(values), 0)
30 
31  key = 3
32  priorVector = np.array([0., 0., 0.])
34  factor = gtsam.PriorFactorVector(key, priorVector, model)
35  values.insert(key, priorVector)
36  self.assertEqual(factor.error(values), 0)
37 
38  def test_AddPrior(self):
39  """
40  Test adding prior factors directly to factor graph via the .addPrior method.
41  """
42  # define factor graph
44 
45  # define and add Pose3 prior
46  key = 5
47  priorPose3 = gtsam.Pose3()
49  graph.addPriorPose3(key, priorPose3, model)
50  self.assertEqual(graph.size(), 1)
51 
52  # define and add Vector prior
53  key = 3
54  priorVector = np.array([0., 0., 0.])
56  graph.addPriorVector(key, priorVector, model)
57  self.assertEqual(graph.size(), 2)
58 
59 
60 if __name__ == "__main__":
61  unittest.main()
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610


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