From 6145466decfe06eaf311dd651ce244cfcc8f92a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Oct 2021 15:02:24 -0400 Subject: [PATCH] add type annotations --- python/gtsam/examples/IMUKittiExampleGPS.py | 30 +++++++++++---------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py index a23f98186..5a9811522 100644 --- a/python/gtsam/examples/IMUKittiExampleGPS.py +++ b/python/gtsam/examples/IMUKittiExampleGPS.py @@ -4,11 +4,11 @@ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BEN Author: Varun Agrawal """ import argparse - -import numpy as np +from typing import List import gtsam -from gtsam import Pose3, noiseModel +import numpy as np +from gtsam import ISAM2, Point3, Pose3, noiseModel from gtsam.symbol_shorthand import B, V, X @@ -31,7 +31,8 @@ class KittiCalibration: class ImuMeasurement: """An instance of an IMU measurement.""" - def __init__(self, time, dt, accelerometer, gyroscope): + def __init__(self, time: float, dt: float, accelerometer: gtsam.Point3, + gyroscope: gtsam.Point3): self.time = time self.dt = dt self.accelerometer = accelerometer @@ -40,14 +41,14 @@ class ImuMeasurement: class GpsMeasurement: """An instance of a GPS measurement.""" - def __init__(self, time, position: gtsam.Point3): + def __init__(self, time: float, position: gtsam.Point3): self.time = time self.position = position -def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", - gps_data_file="KittiGps_converted.txt", - imu_metadata_file="KittiEquivBiasedImu_metadata.txt"): +def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt", + gps_data_file: str = "KittiGps_converted.txt", + imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"): """ Load the KITTI Dataset. """ @@ -56,7 +57,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma # AverageDeltaT imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) - with open(imu_metadata_file) as imu_metadata: + with open(imu_metadata_file, encoding='UTF-8') as imu_metadata: print("-- Reading sensor metadata") line = imu_metadata.readline() # Ignore the first line line = imu_metadata.readline().strip() @@ -70,7 +71,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", imu_measurements = [] print("-- Reading IMU measurements from file") - with open(imu_data_file) as imu_data: + with open(imu_data_file, encoding='UTF-8') as imu_data: data = imu_data.readlines() for i in range(1, len(data)): # ignore the first line time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map( @@ -86,7 +87,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", gps_measurements = [] print("-- Reading GPS measurements from file") - with open(gps_data_file) as gps_data: + with open(gps_data_file, encoding='UTF-8') as gps_data: data = gps_data.readlines() for i in range(1, len(data)): time, x, y, z = map(float, data[i].split(',')) @@ -96,7 +97,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", return kitti_calibration, imu_measurements, gps_measurements -def getImuParams(kitti_calibration): +def getImuParams(kitti_calibration: KittiCalibration): """Get the IMU parameters from the KITTI calibration data.""" GRAVITY = 9.8 w_coriolis = np.zeros(3) @@ -122,11 +123,12 @@ def getImuParams(kitti_calibration): return imu_params -def save_results(isam, output_filename, first_gps_pose, gps_measurements): +def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, + gps_measurements: List): """Write the results from `isam` to `output_filename`.""" # Save results to file print("Writing results to file...") - with open(output_filename, 'w') as fp_out: + with open(output_filename, 'w', encoding='UTF-8') as fp_out: fp_out.write( "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n")