ShonanAveragingCLI.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 Shonan Rotation Averaging CLI reads a *pose* graph, extracts the
10 rotation constraints, and runs the Shonan algorithm.
11 
12 Author: Frank Dellaert
13 Date: August 2020
14 """
15 # pylint: disable=invalid-name, E1101
16 
17 import argparse
18 
19 import matplotlib.pyplot as plt
20 import numpy as np
21 
22 import gtsam
23 from gtsam.utils import plot
24 
25 def estimate_poses_given_rot(factors: gtsam.BetweenFactorPose3s,
26  rotations: gtsam.Values,
27  d: int = 3):
28  """ Estimate Poses from measurements, given rotations. From SfmProblem in shonan.
29 
30  Arguments:
31  factors -- data structure with many BetweenFactorPose3 factors
32  rotations {Values} -- Estimated rotations
33 
34  Returns:
35  Values -- Estimated Poses
36  """
37 
38  I_d = np.eye(d)
39 
40  def R(j):
41  return rotations.atRot3(j) if d == 3 else rotations.atRot2(j)
42 
43  def pose(R, t):
44  return gtsam.Pose3(R, t) if d == 3 else gtsam.Pose2(R, t)
45 
48 
49  # Add a factor anchoring t_0
50  graph.add(0, I_d, np.zeros((d,)), model)
51 
52  # Add a factor saying t_j - t_i = Ri*t_ij for all edges (i,j)
53  for factor in factors:
54  keys = factor.keys()
55  i, j, Tij = keys[0], keys[1], factor.measured()
56  measured = R(i).rotate(Tij.translation())
57  graph.add(j, I_d, i, -I_d, measured, model)
58 
59  # Solve linear system
60  translations = graph.optimize()
61 
62  # Convert to Values.
63  result = gtsam.Values()
64  for j in range(rotations.size()):
65  tj = translations.at(j)
66  result.insert(j, pose(R(j), tj))
67 
68  return result
69 
70 
71 def run(args):
72  """Run Shonan averaging and then recover translations linearly before saving result."""
73 
74  # Get input file
75  if args.input_file:
76  input_file = args.input_file
77  else:
78  if args.named_dataset == "":
79  raise ValueError(
80  "You must either specify a named dataset or an input file")
81  input_file = gtsam.findExampleDataFile(args.named_dataset)
82 
83  if args.dimension == 2:
84  print("Running Shonan averaging for SO(2) on ", input_file)
85  shonan = gtsam.ShonanAveraging2(input_file)
86  if shonan.nrUnknowns() == 0:
87  raise ValueError("No 2D pose constraints found, try -d 3.")
88  initial = shonan.initializeRandomly()
89  rotations, _ = shonan.run(initial, 2, 10)
90  factors = gtsam.parse2DFactors(input_file)
91  elif args.dimension == 3:
92  print("Running Shonan averaging for SO(3) on ", input_file)
93  shonan = gtsam.ShonanAveraging3(input_file)
94  if shonan.nrUnknowns() == 0:
95  raise ValueError("No 3D pose constraints found, try -d 2.")
96  initial = shonan.initializeRandomly()
97  rotations, _ = shonan.run(initial, 3, 10)
98  factors = gtsam.parse3DFactors(input_file)
99  else:
100  raise ValueError("Can only run SO(2) or SO(3) averaging")
101 
102  print("Recovering translations")
103  poses = estimate_poses_given_rot(factors, rotations, args.dimension)
104 
105  print("Writing result to ", args.output_file)
106  gtsam.writeG2o(gtsam.NonlinearFactorGraph(), poses, args.output_file)
107 
108  plot.plot_trajectory(1, poses, scale=0.2)
109  plot.set_axes_equal(1)
110  plt.show()
111 
112 
113 if __name__ == "__main__":
114  parser = argparse.ArgumentParser()
115  parser.add_argument('-n', '--named_dataset', type=str, default="pose3example-grid",
116  help='Find and read frome example dataset file')
117  parser.add_argument('-i', '--input_file', type=str, default="",
118  help='Read pose constraints graph from the specified file')
119  parser.add_argument('-o', '--output_file', type=str, default="shonan.g2o",
120  help='Write solution to the specified file')
121  parser.add_argument('-d', '--dimension', type=int, default=3,
122  help='Optimize over 2D or 3D rotations')
123  parser.add_argument("-p", "--plot", action="store_true", default=True,
124  help="Plot result")
125  run(parser.parse_args())
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:636
BetweenFactorPose3s parse3DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:954
Rot2 R(Rot2::fromAngle(0.1))
BetweenFactorPose2s parse2DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:948
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Double_ range(const Point2_ &p, const Point2_ &q)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42