TranslationAveragingExample.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 This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery)
10 together for estimating global translations from relative translation directions and global rotations.
11 The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset.
12 
13 Author: Akshay Krishnan
14 Date: September 2020
15 """
16 
17 from collections import defaultdict
18 from typing import List, Tuple
19 
20 import gtsam
21 import numpy as np
22 from gtsam.examples import SFMdata
23 
24 # Hyperparameters for 1dsfm, values used from Kyle Wilson's code.
25 MAX_1DSFM_PROJECTION_DIRECTIONS = 48
26 OUTLIER_WEIGHT_THRESHOLD = 0.1
27 
28 
29 def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
30  """"Returns global rotations and unit translation directions between 8 cameras
31  that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
32  and the unit translations directions between some camera pairs are computed from their
33  global translations. """
34  wTc_list = SFMdata.createPoses()
35  # Rotations of the cameras in the world frame.
36  wRc_values = gtsam.Values()
37  # Normalized translation directions from camera i to camera j
38  # in the coordinate frame of camera i.
39  i_iZj_list = []
40  for i in range(0, len(wTc_list) - 2):
41  # Add the rotation.
42  wRi = wTc_list[i].rotation()
43  wRc_values.insert(i, wRi)
44  # Create unit translation measurements with next two poses.
45  for j in range(i + 1, i + 3):
46  # Compute the translation from pose i to pose j, in the world reference frame.
47  w_itj = wTc_list[j].translation() - wTc_list[i].translation()
48  # Obtain the translation in the camera i's reference frame.
49  i_itj = wRi.unrotate(w_itj)
50  # Compute the normalized unit translation direction.
51  i_iZj = gtsam.Unit3(i_itj)
52  i_iZj_list.append(gtsam.BinaryMeasurementUnit3(
53  i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
54  # Add the last two rotations.
55  wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation())
56  wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation())
57  return wRc_values, i_iZj_list
58 
59 
60 def filter_outliers(w_iZj_list: List[gtsam.BinaryMeasurementUnit3]) -> List[gtsam.BinaryMeasurementUnit3]:
61  """Removes outliers from a list of Unit3 measurements that are the
62  translation directions from camera i to camera j in the world frame."""
63 
64  # Indices of measurements that are to be used as projection directions.
65  # These are randomly chosen. All sampled directions must be unique.
66  num_directions_to_sample = min(
67  MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list))
68  sampled_indices = np.random.choice(
69  len(w_iZj_list), num_directions_to_sample, replace=False)
70 
71  # Sample projection directions from the measurements.
72  projection_directions = [w_iZj_list[idx].measured()
73  for idx in sampled_indices]
74 
75  outlier_weights = []
76  # Find the outlier weights for each direction using MFAS.
77  for direction in projection_directions:
78  algorithm = gtsam.MFAS(w_iZj_list, direction)
79  outlier_weights.append(algorithm.computeOutlierWeights())
80 
81  # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys
82  # (camera IDs) to a weight, where weights are proportional to the probability of the edge
83  # being an outlier.
84  avg_outlier_weights = defaultdict(float)
85  for outlier_weight_dict in outlier_weights:
86  for keypair, weight in outlier_weight_dict.items():
87  avg_outlier_weights[keypair] += weight / len(outlier_weights)
88 
89  # Remove w_iZj that have weight greater than threshold, these are outliers.
90  w_iZj_inliers = []
91  [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[(
92  w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD]
93 
94  return w_iZj_inliers
95 
96 
97 def estimate_poses(i_iZj_list: List[gtsam.BinaryMeasurementUnit3],
98  wRc_values: gtsam.Values) -> gtsam.Values:
99  """Estimate poses given rotations and normalized translation directions between cameras.
100 
101  Args:
102  i_iZj_list: List of normalized translation direction measurements between camera pairs,
103  Z here refers to measurements. The measurements are of camera j with reference
104  to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit
105  vector to j in i's frame and is not a transformation.
106  wRc_values: Rotations of the cameras in the world frame.
107 
108  Returns:
109  gtsam.Values: Estimated poses.
110  """
111 
112  # Convert the translation direction measurements to world frame using the rotations.
113  w_iZj_list = []
114  for i_iZj in i_iZj_list:
115  w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1())
116  .rotate(i_iZj.measured().point3()))
117  w_iZj_list.append(gtsam.BinaryMeasurementUnit3(
118  i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel()))
119 
120  # Remove the outliers in the unit translation directions.
121  w_iZj_inliers = filter_outliers(w_iZj_list)
122 
123  # Run the optimizer to obtain translations for normalized directions.
124  wtc_values = gtsam.TranslationRecovery().run(w_iZj_inliers)
125 
126  wTc_values = gtsam.Values()
127  for key in wRc_values.keys():
128  wTc_values.insert(key, gtsam.Pose3(
129  wRc_values.atRot3(key), wtc_values.atPoint3(key)))
130  return wTc_values
131 
132 
133 def main():
134  wRc_values, i_iZj_list = get_data()
135  wTc_values = estimate_poses(i_iZj_list, wRc_values)
136  print("**** Translation averaging output ****")
137  print(wTc_values)
138  print("**************************************")
139 
140 
141 if __name__ == '__main__':
142  main()
gtsam::TranslationRecovery
Definition: TranslationRecovery.h:51
gtsam::translation
Point3_ translation(const Pose3_ &pose)
Definition: slam/expressions.h:93
gtsam::MFAS
Definition: MFAS.h:51
gtsam.examples.TranslationAveragingExample.filter_outliers
List[gtsam.BinaryMeasurementUnit3] filter_outliers(List[gtsam.BinaryMeasurementUnit3] w_iZj_list)
Definition: TranslationAveragingExample.py:60
gtsam::point3
Point3_ point3(const Unit3_ &v)
Definition: slam/expressions.h:101
measured
Point2 measured(-17, 30)
gtsam.examples
Definition: python/gtsam/examples/__init__.py:1
gtsam.examples.TranslationAveragingExample.estimate_poses
gtsam.Values estimate_poses(List[gtsam.BinaryMeasurementUnit3] i_iZj_list, gtsam.Values wRc_values)
Definition: TranslationAveragingExample.py:97
gtsam.examples.TranslationAveragingExample.get_data
Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]] get_data()
Definition: TranslationAveragingExample.py:29
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam::rotate
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:97
gtsam::Pose3
Definition: Pose3.h:37
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::Values
Definition: Values.h:65
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:625
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2446
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam.examples.TranslationAveragingExample.main
def main()
Definition: TranslationAveragingExample.py:133


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:18:00