12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

adding documentation for example

release/4.3a0
akrishnan86 2020-09-21 20:40:43 -07:00
parent 565467f2ff
commit 4b06616dfe
1 changed files with 34 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
python/gtsam/examples/TranslationAveragingExample.py
View File

@ -1,17 +1,27 @@
from collections import Counter """
import functools GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
import operator Atlanta, Georgia 30332-0415
All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery)
together for estimating global translations from relative translation directions and global rotations.
The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset.
Author: Akshay Krishnan
Date: September 2020
"""
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam.examples import SFMdata from gtsam.examples import SFMdata
max_1dsfm_projection_directions = 50
outlier_weight_threshold = 0.1
def get_data(): def get_data():
""""Returns data from SfMData.createPoses(). This contains the global rotations and the unit translations directions.""" """"Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions."""
# Using toy dataset in SfMdata for example. # Using toy dataset in SfMdata for example.
poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)) poses = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0))
rotations = gtsam.Values() rotations = gtsam.Values()
@ -21,7 +31,8 @@ def get_data():
rotations.insert(i, poses[i].rotation()) rotations.insert(i, poses[i].rotation())
# Create unit translation measurements with next two poses # Create unit translation measurements with next two poses
for j in range(i + 1, i + 3): for j in range(i + 1, i + 3):
i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(poses[j].translation() - poses[i].translation())) i_Z_j = gtsam.Unit3(poses[i].rotation().unrotate(
poses[j].translation() - poses[i].translation()))
translation_directions.append(gtsam.BinaryMeasurementUnit3( translation_directions.append(gtsam.BinaryMeasurementUnit3(
i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
# Add the last two rotations. # Add the last two rotations.
@ -35,18 +46,23 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3,
"""Estimate poses given normalized translation directions and rotations between nodes. """Estimate poses given normalized translation directions and rotations between nodes.
Arguments: Arguments:
measurements - List of translation direction from the first node to the second node in the coordinate frame of the first node. measurements {BinaryMeasurementsUnit3}- List of translation direction from the first node to
the second node in the coordinate frame of the first node.
rotations {Values} -- Estimated rotations rotations {Values} -- Estimated rotations
Returns: Returns:
Values -- Estimated poses. Values -- Estimated poses.
""" """
# Some hyperparameters.
max_1dsfm_projection_directions = 50
outlier_weight_threshold = 0.1
# Convert the translation directions to global frame using the rotations. # Convert the translation directions to global frame using the rotations.
w_measurements = gtsam.BinaryMeasurementsUnit3() w_measurements = gtsam.BinaryMeasurementsUnit3()
for measurement in measurements: for measurement in measurements:
w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2( w_measurements.append(gtsam.BinaryMeasurementUnit3(measurement.key1(), measurement.key2(), gtsam.Unit3(
), gtsam.Unit3(rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel())) rotations.atRot3(measurement.key1()).rotate(measurement.measured().point3())), measurement.noiseModel()))
# Indices of measurements that are to be used as projection directions. These are randomly chosen. # Indices of measurements that are to be used as projection directions. These are randomly chosen.
indices = np.random.choice(len(w_measurements), min( indices = np.random.choice(len(w_measurements), min(
@ -71,7 +87,8 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3,
# Remove measurements that have weight greater than threshold. # Remove measurements that have weight greater than threshold.
inlier_measurements = gtsam.BinaryMeasurementsUnit3() inlier_measurements = gtsam.BinaryMeasurementsUnit3()
[inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[(m.key1(), m.key2())] < outlier_weight_threshold] [inlier_measurements.append(m) for m in w_measurements if avg_outlier_weights[(
m.key1(), m.key2())] < outlier_weight_threshold]
# Run the optimizer to obtain translations for normalized directions. # Run the optimizer to obtain translations for normalized directions.
translations = gtsam.TranslationRecovery(inlier_measurements).run() translations = gtsam.TranslationRecovery(inlier_measurements).run()
@ -82,6 +99,7 @@ def estimate_poses_given_rot(measurements: gtsam.BinaryMeasurementsUnit3,
rotations.atRot3(key), translations.atPoint3(key))) rotations.atRot3(key), translations.atPoint3(key)))
return poses return poses
def main(): def main():
rotations, translation_directions = get_data() rotations, translation_directions = get_data()
poses = estimate_poses_given_rot(translation_directions, rotations) poses = estimate_poses_given_rot(translation_directions, rotations)
@ -89,5 +107,6 @@ def main():
print(poses) print(poses)
print("**************************************") print("**************************************")
if __name__ == '__main__': if __name__ == '__main__':
main() main()