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

add type annotations

release/4.3a0
Varun Agrawal 2021-10-21 15:02:24 -04:00
parent 23858f31e9
commit 6145466dec
1 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
python/gtsam/examples/IMUKittiExampleGPS.py
View File

@ -4,11 +4,11 @@ Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BEN
Author: Varun Agrawal Author: Varun Agrawal
""" """
import argparse import argparse
from typing import List
import numpy as np
import gtsam 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 from gtsam.symbol_shorthand import B, V, X
@ -31,7 +31,8 @@ class KittiCalibration:
class ImuMeasurement: class ImuMeasurement:
"""An instance of an IMU measurement.""" """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.time = time
self.dt = dt self.dt = dt
self.accelerometer = accelerometer self.accelerometer = accelerometer
@ -40,14 +41,14 @@ class ImuMeasurement:
class GpsMeasurement: class GpsMeasurement:
"""An instance of a GPS measurement.""" """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.time = time
self.position = position self.position = position
def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt", def loadKittiData(imu_data_file: str = "KittiEquivBiasedImu.txt",
gps_data_file="KittiGps_converted.txt", gps_data_file: str = "KittiGps_converted.txt",
imu_metadata_file="KittiEquivBiasedImu_metadata.txt"): imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"):
""" """
Load the KITTI Dataset. Load the KITTI Dataset.
""" """
@ -56,7 +57,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt",
# GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma # GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
# AverageDeltaT # AverageDeltaT
imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file) 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") print("-- Reading sensor metadata")
line = imu_metadata.readline() # Ignore the first line line = imu_metadata.readline() # Ignore the first line
line = imu_metadata.readline().strip() line = imu_metadata.readline().strip()
@ -70,7 +71,7 @@ def loadKittiData(imu_data_file="KittiEquivBiasedImu.txt",
imu_measurements = [] imu_measurements = []
print("-- Reading IMU measurements from file") 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() data = imu_data.readlines()
for i in range(1, len(data)): # ignore the first line 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( 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 = [] gps_measurements = []
print("-- Reading GPS measurements from file") 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() data = gps_data.readlines()
for i in range(1, len(data)): for i in range(1, len(data)):
time, x, y, z = map(float, data[i].split(',')) 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 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.""" """Get the IMU parameters from the KITTI calibration data."""
GRAVITY = 9.8 GRAVITY = 9.8
w_coriolis = np.zeros(3) w_coriolis = np.zeros(3)
@ -122,11 +123,12 @@ def getImuParams(kitti_calibration):
return imu_params 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`.""" """Write the results from `isam` to `output_filename`."""
# Save results to file # Save results to file
print("Writing 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( 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") "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n")