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

renaming variables

release/4.3a0
akrishnan86 2020-09-30 23:25:20 -07:00
parent 98404ad27e
commit 634682738e
1 changed files with 60 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -22,99 +22,103 @@ import numpy as np
import gtsam import gtsam
from gtsam.examples import SFMdata from gtsam.examples import SFMdata
# Hyperparameters for 1dsfm, values used from Kyle Wilson's code.
MAX_1DSFM_PROJECTION_DIRECTIONS = 48
OUTLIER_WEIGHT_THRESHOLD = 0.1
def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
""""Returns data from SfMData.createPoses(). This contains global rotations and unit translations directions.""" """"Returns global rotations and unit translation directions between 8 cameras
that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
and the unit translations directions between some camera pairs are computed from their
global translations. """
# 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)) wTc = SFMdata.createPoses(gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0))
# Rotations of the cameras in the world frame - wRc. # Rotations of the cameras in the world frame.
rotations = gtsam.Values() wRc_values = gtsam.Values()
# Normalized translation directions for pairs of cameras - from first camera to second, # Normalized translation directions from camera i to camera j
# in the coordinate frame of the first camera. # in the coordinate frame of camera i.
translation_directions = [] i_iZj_list = []
for i in range(0, len(poses) - 2): for i in range(0, len(wTc) - 2):
# Add the rotation. # Add the rotation.
rotations.insert(i, poses[i].rotation()) wRc_values.insert(i, wTc[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( i_iZj = gtsam.Unit3(wTc[i].rotation().unrotate(
poses[j].translation() - poses[i].translation())) wTc[j].translation() - wTc[i].translation()))
translation_directions.append(gtsam.BinaryMeasurementUnit3( i_iZj_list.append(gtsam.BinaryMeasurementUnit3(
i, j, i_Z_j, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
# Add the last two rotations. # Add the last two rotations.
rotations.insert(len(poses) - 1, poses[-1].rotation()) wRc_values.insert(len(wTc) - 1, wTc[-1].rotation())
rotations.insert(len(poses) - 2, poses[-2].rotation()) wRc_values.insert(len(wTc) - 2, wTc[-2].rotation())
return (rotations, translation_directions) return (wRc_values, i_iZj_list)
def estimate_poses(relative_translations: gtsam.BinaryMeasurementsUnit3, def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
rotations: gtsam.Values) -> gtsam.Values: wRc_values: gtsam.Values) -> gtsam.Values:
"""Estimate poses given rotations normalized translation directions between cameras. """Estimate poses given rotations and normalized translation directions between cameras.
Args: Args:
relative_translations -- List of normalized translation directions between camera pairs, each direction iZj_list -- List of normalized translation direction measurements between camera pairs,
is from the first camera to the second, in the frame of the first camera. Z here refers to measurements. The measurements are of camera j with reference
rotations -- Rotations of the cameras in the world frame. to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit
vector to j in i's frame and is not a transformation.
wRc_values -- Rotations of the cameras in the world frame.
Returns: Returns:
Values -- Estimated poses. Values -- Estimated poses.
""" """
# Some hyperparameters, values used from 1dsfm. # Convert the translation direction measurements to world frame using the rotations.
max_1dsfm_projection_directions = 48 w_iZj_list = gtsam.BinaryMeasurementsUnit3()
outlier_weight_threshold = 0.1 for i_iZj in i_iZj_list:
w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1())
.rotate(i_iZj.measured().point3()))
w_iZj_list.append(gtsam.BinaryMeasurementUnit3(
i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel()))
# Convert the translation directions to global frame using the rotations. # Indices of measurements that are to be used as projection directions.
w_relative_translations = gtsam.BinaryMeasurementsUnit3() # These are randomly chosen.
for relative_translation in relative_translations: sampled_indices = np.random.choice(len(w_iZj_list), min(
w_relative_translation = gtsam.Unit3(rotations.atRot3(relative_translation.key1()) MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)), replace=False)
.rotate(relative_translation.measured().point3()))
w_relative_translations.append(gtsam.BinaryMeasurementUnit3(relative_translation.key1(),
relative_translation.key2(),
w_relative_translation,
relative_translation.noiseModel()))
# Indices of measurements that are to be used as projection directions. These are randomly chosen.
sampled_indices = np.random.choice(len(w_relative_translations), min(
max_1dsfm_projection_directions, len(w_relative_translations)), replace=False)
# Sample projection directions from the measurements. # Sample projection directions from the measurements.
projection_directions = [ projection_directions = [w_iZj_list[idx].measured()
w_relative_translations[idx].measured() for idx in sampled_indices] for idx in sampled_indices]
outlier_weights = [] outlier_weights = []
# Find the outlier weights for each direction using MFAS. # Find the outlier weights for each direction using MFAS.
for direction in projection_directions: for direction in projection_directions:
algorithm = gtsam.MFAS(w_relative_translations, direction) algorithm = gtsam.MFAS(w_iZj_list, direction)
outlier_weights.append(algorithm.computeOutlierWeights()) outlier_weights.append(algorithm.computeOutlierWeights())
# Compute average of outlier weights. Each outlier weight is a map from a pair of Keys (camera IDs) to a weight, # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys
# where weights are proportional to the probability of the edge being an outlier. # (camera IDs) to a weight, where weights are proportional to the probability of the edge
# being an outlier.
avg_outlier_weights = defaultdict(float) avg_outlier_weights = defaultdict(float)
for outlier_weight_dict in outlier_weights: for outlier_weight_dict in outlier_weights:
for keypair, weight in outlier_weight_dict.items(): for keypair, weight in outlier_weight_dict.items():
avg_outlier_weights[keypair] += weight / len(outlier_weights) avg_outlier_weights[keypair] += weight / len(outlier_weights)
# Remove w_relative_tranlsations that have weight greater than threshold, these are outliers. # Remove w_relative_tranlsations that have weight greater than threshold, these are outliers.
inlier_w_relative_translations = gtsam.BinaryMeasurementsUnit3() w_iZj_inliers = gtsam.BinaryMeasurementsUnit3()
[inlier_w_relative_translations.append(Z) for Z in w_relative_translations [w_iZj_inliers.append(Z) for Z in w_iZj_list
if avg_outlier_weights[(Z.key1(), Z.key2())] < outlier_weight_threshold] if avg_outlier_weights[(Z.key1(), Z.key2())] < OUTLIER_WEIGHT_THRESHOLD]
# Run the optimizer to obtain translations for normalized directions. # Run the optimizer to obtain translations for normalized directions.
w_translations = gtsam.TranslationRecovery( wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
inlier_w_relative_translations).run()
poses = gtsam.Values() wTc_values = gtsam.Values()
for key in rotations.keys(): for key in wRc_values.keys():
poses.insert(key, gtsam.Pose3( wTc_values.insert(key, gtsam.Pose3(
rotations.atRot3(key), w_translations.atPoint3(key))) wRc_values.atRot3(key), wtc_values.atPoint3(key)))
return poses return wTc_values
def main(): def main():
rotations, translation_directions = get_data() wRc_values, w_iZj_list = get_data()
poses = estimate_poses(translation_directions, rotations) wTc_values = estimate_poses(w_iZj_list, wRc_values)
print("**** Translation averaging output ****") print("**** Translation averaging output ****")
print(poses) print(wTc_values)
print("**************************************") print("**************************************")