visual_isam.py
Go to the documentation of this file.
1 import gtsam
2 from gtsam import symbol
3 
4 
5 class Options:
6  """ Options for visual isam example. """
7 
8  def __init__(self):
9  self.hardConstraint = False
10  self.pointPriors = False
11  self.batchInitialization = True
12  self.reorderInterval = 10
13  self.alwaysRelinearize = False
14 
15 
16 def initialize(data, truth, options):
17  # Initialize iSAM
18  params = gtsam.ISAM2Params()
19  if options.alwaysRelinearize:
20  params.setRelinearizeSkip(1)
21  isam = gtsam.ISAM2(params=params)
22 
23  # Add constraints/priors
24  # TODO: should not be from ground truth!
25  newFactors = gtsam.NonlinearFactorGraph()
26  initialEstimates = gtsam.Values()
27  for i in range(2):
28  ii = symbol('x', i)
29  if i == 0:
30  if options.hardConstraint: # add hard constraint
31  newFactors.add(
32  gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
33  else:
34  newFactors.add(
35  gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
36  data.noiseModels.posePrior))
37  initialEstimates.insert(ii, truth.cameras[i].pose())
38 
39  nextPoseIndex = 2
40 
41  # Add visual measurement factors from two first poses and initialize
42  # observed landmarks
43  for i in range(2):
44  ii = symbol('x', i)
45  for k in range(len(data.Z[i])):
46  j = data.J[i][k]
47  jj = symbol('l', j)
48  newFactors.add(
49  gtsam.GenericProjectionFactorCal3_S2(data.Z[i][
50  k], data.noiseModels.measurement, ii, jj, data.K))
51  # TODO: initial estimates should not be from ground truth!
52  if not initialEstimates.exists(jj):
53  initialEstimates.insert(jj, truth.points[j])
54  if options.pointPriors: # add point priors
55  newFactors.add(
56  gtsam.PriorFactorPoint3(jj, truth.points[j],
57  data.noiseModels.pointPrior))
58 
59  # Add odometry between frames 0 and 1
60  newFactors.add(
61  gtsam.BetweenFactorPose3(
62  symbol('x', 0),
63  symbol('x', 1), data.odometry[1], data.noiseModels.odometry))
64 
65  # Update ISAM
66  if options.batchInitialization: # Do a full optimize for first two poses
67  batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors,
68  initialEstimates)
69  fullyOptimized = batchOptimizer.optimize()
70  isam.update(newFactors, fullyOptimized)
71  else:
72  isam.update(newFactors, initialEstimates)
73 
74  # figure(1)tic
75  # t=toc plot(frame_i,t,'r.') tic
76  result = isam.calculateEstimate()
77  # t=toc plot(frame_i,t,'g.')
78 
79  return isam, result, nextPoseIndex
80 
81 
82 def step(data, isam, result, truth, currPoseIndex):
83  '''
84  Do one step isam update
85  @param[in] data: measurement data (odometry and visual measurements and their noiseModels)
86  @param[in/out] isam: current isam object, will be updated
87  @param[in/out] result: current result object, will be updated
88  @param[in] truth: ground truth data, used to initialize new variables
89  @param[currPoseIndex]: index of the current pose
90  '''
91  # iSAM expects us to give it a new set of factors
92  # along with initial estimates for any new variables introduced.
93  newFactors = gtsam.NonlinearFactorGraph()
94  initialEstimates = gtsam.Values()
95 
96  # Add odometry
97  prevPoseIndex = currPoseIndex - 1
98  odometry = data.odometry[prevPoseIndex]
99  newFactors.add(
100  gtsam.BetweenFactorPose3(
101  symbol('x', prevPoseIndex),
102  symbol('x', currPoseIndex), odometry,
103  data.noiseModels.odometry))
104 
105  # Add visual measurement factors and initializations as necessary
106  for k in range(len(data.Z[currPoseIndex])):
107  zij = data.Z[currPoseIndex][k]
108  j = data.J[currPoseIndex][k]
109  jj = symbol('l', j)
110  newFactors.add(
111  gtsam.GenericProjectionFactorCal3_S2(
112  zij, data.noiseModels.measurement,
113  symbol('x', currPoseIndex), jj, data.K))
114  # TODO: initialize with something other than truth
115  if not result.exists(jj) and not initialEstimates.exists(jj):
116  lmInit = truth.points[j]
117  initialEstimates.insert(jj, lmInit)
118 
119  # Initial estimates for the new pose.
120  prevPose = result.atPose3(symbol('x', prevPoseIndex))
121  initialEstimates.insert(
122  symbol('x', currPoseIndex), prevPose.compose(odometry))
123 
124  # Update ISAM
125  # figure(1)tic
126  isam.update(newFactors, initialEstimates)
127  # t=toc plot(frame_i,t,'r.') tic
128  newResult = isam.calculateEstimate()
129  # t=toc plot(frame_i,t,'g.')
130 
131  return isam, newResult
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
def initialize(data, truth, options)
Definition: visual_isam.py:16
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Key symbol(unsigned char c, std::uint64_t j)
size_t len(handle h)
Definition: pytypes.h:1514


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:51:25