IMUKittiExampleGPS Python Example #836

varunagrawal · 2021-08-01T12:26:47Z

Fixes #566

varunagrawal · 2021-10-21T17:01:25Z

@ProfFan this should be an easy review, since the C++ code is just formatting changes and there is a 1-to-1 correspondence between the Python and C++ examples.

johnwlambert · 2021-10-21T17:07:49Z

python/gtsam/examples/IMUKittiExampleGPS.py

+    return imu_params
+
+
+def save_results(isam, output_filename, first_gps_pose, gps_measurements):


Thanks for the PR, Varun. From a quick glance, looks like we need type annotations here on this function signature

Thanks! Updated.

johnwlambert · 2021-10-21T19:07:03Z

python/gtsam/examples/IMUKittiExampleGPS.py

+
+
+def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int,
+                 gps_measurements: List):


thanks for those updates @varunagrawal. Here the type is List[something] -- what's the inner entry?

Ah I got distracted by something and missed this. Thanks!

johnwlambert · 2021-10-21T19:07:59Z

python/gtsam/examples/IMUKittiExampleGPS.py

+                current_bias = result.atConstantBias(current_bias_key)
+
+                print(
+                    "################ POSE AT TIME {} ################".format(


maybe more readable as a Python f-string? (and would be on one-line)

ProfFan

LGTM

johnwlambert · 2021-10-21T19:20:15Z

python/gtsam/examples/IMUKittiExampleGPS.py

+    args = parse_args()
+    kitti_calibration, imu_measurements, gps_measurements = loadKittiData()
+
+    if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8):


the code looks good. I think the main method is about 161 lines though, in length. So I would vote to modularize it a bit before checking it in. However, I do understand that the original C++ code main function was also very long, so not your fault in any way

johnwlambert · 2021-10-21T19:20:54Z

python/gtsam/examples/IMUKittiExampleGPS.py

+                gps[0], gps[1], gps[2]))
+
+
+def parse_args():


missing return type here

python/gtsam/examples/IMUKittiExampleGPS.py

johnwlambert · 2021-10-21T19:25:01Z

python/gtsam/examples/IMUKittiExampleGPS.py

+
+
+class ImuMeasurement:
+    """An instance of an IMU measurement."""


I would make this a dataclass, since it functions as one:

from dataclasses import dataclass @dataclass class ImuMeasurement: """An instance of an IMU measurement.""" time: float dt: float accelerometer: gtsam.Point3 gyroscope: gtsam.Point3

less boilerplate and more readable IMHO, for the same functionality

We can't do dataclasses since it is a python 3.7 feature and we need to support python 3.6.

johnwlambert · 2021-10-21T19:25:22Z

python/gtsam/examples/IMUKittiExampleGPS.py

+
+class GpsMeasurement:
+    """An instance of a GPS measurement."""
+    def __init__(self, time: float, position: gtsam.Point3):


same story here -- prefer a dataclass

Need to support python 3.6 sadly. I use dataclasses all the time in my own projects and I much prefer them...

Python 3.6 is end of life in 2 months though -- https://devguide.python.org/#status-of-python-branches
https://endoflife.date/python

I think security fixes aren't supported after that date, so it could be insecure to use. Any reason to keep supporting 3.6?

The GTSAM user base. Some of them are companies (which take forever to update their software infra) and we would need Frank's express approval to make this update.

But anyway, this is not directly related to the functionality of this PR (more of a nice-to-have) so we can discuss this later. 🙂

johnwlambert · 2021-10-21T20:43:30Z

python/gtsam/examples/IMUKittiExampleGPS.py

+                 accelerometer_sigma: float, gyroscope_sigma: float,
+                 integration_sigma: float, accelerometer_bias_sigma: float,
+                 gyroscope_bias_sigma: float, average_delta_t: float):
+        self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz),


didn't follow how this is equivalent to body_T_imu = Pose3::Expmap(BodyP);

Pose3::Expmap is converting (rx, ry, rz, tx, ty, tz) to a Pose3 object. I am simply reading the values in directly since we don't need the exponential map operation. This makes it more readable as well.

@varunagrawal I might be slow understanding this, but maybe I'm missing the relationship between the Euler angles here and the exponential map. A quick example seems to show these are not equivalent:

>>> import gtsam >>> from gtsam import Rot3, Pose3 >>> import numpy as np >>> Pose3.Expmap(np.array([ np.deg2rad(45), np.deg2rad(50), np.deg2rad(55), 0.1, 0.2, 0.3 ]) ) R: [ 0.307904, -0.350198, 0.884622; 0.913896, 0.367406, -0.172647; -0.264554, 0.861611, 0.43317 ] t: 0.157663 0.144392 0.303374 >>> Pose3(gtsam.Rot3.RzRyRx(np.deg2rad(45), np.deg2rad(50), np.deg2rad(55)), gtsam.Point3(0.1, 0.2, 0.3) ) R: [ 0.368688, -0.268536, 0.88992; 0.526541, 0.849294, 0.0381346; -0.766044, 0.454519, 0.454519 ] t: 0.1 0.2 0.3 >>>

My bad, I should have prefaced by saying that the bodyP values are near identity for rotation, so the two are equivalent over there. I don't know why @mindThomas decided to use an Expmap there when he first wrote the example.

johnwlambert · 2021-10-21T20:51:35Z

python/gtsam/examples/IMUKittiExampleGPS.py

+                    sigma_init_v, sigma_init_b, noise_model_gps,
+                    kitti_calibration, first_gps_pose, gps_skip)
+
+    save_results(isam, args.output_filename, first_gps_pose, gps_measurements)


@varunagrawal is the python output the same as the c++ output?

I verified this back when I made the PR. I'll generate the outputs again and run the diff.

Here's the diff https://www.diffchecker.com/7QT5mumN

varunagrawal · 2021-10-22T12:11:17Z

Thanks for the detailed reviews @johnwlambert and @ProfFan! We're one step closer to a release.

cxwoaiwojiaa · 2021-11-09T07:28:09Z

Hello, nice to meet you, and thank you very much for providing such a useful example for new users of gtsam. About the program (imukittiexamplegps. M), I have successfully run it. Now I want to test it with different data. As a first step, I try to generate data in the same format from Kitti raw data. I want to know Kittiequivbiasedimu.txt and attached Kittigps_converted.txt what do these two data components represent respectively, such as attached What are the meanings of X, y and Z in kittigps_converted.txt? What are the meanings of X, y and Z in kittiequivbiasedimu.txt. Looking forward to your reply!!!
您好，很高兴见到您，非常感谢您为 gts am 的新用户提供了这样一个有用的示例。关于程序（ imu kitti example gps 。米），我已经成功运行了。现在我想用不同的数据来检验。作为第一步，我尝试从 Kitti 原始数据生成相同格式的数据。我想知道 Kitti equiv bia sedimum . txt 和附件中的 Kitt i gps _ convorted . txt 这两个数据分量分别代表什么，如附件中 kitt iq ps _ Convented . txt 中的 X ， y ， Z 是什么意思？kitti equiv biased imu . txt 中的 X 、 y 、 Z 是什么意思？期待您的回复！！！

first pass at IMUKittiExampleGPS.py

f6a4329

varunagrawal self-assigned this Aug 1, 2021

varunagrawal changed the title ~~IMUKittiExampleGPS Python Examples~~ IMUKittiExampleGPS Python Example Aug 1, 2021

varunagrawal added 2 commits August 20, 2021 13:33

only format c++ file (no code changes)

a4a58cf

working implementation

23858f3

varunagrawal requested a review from ProfFan August 20, 2021 17:40

varunagrawal marked this pull request as ready for review August 20, 2021 17:40

varunagrawal added the enhancement Improvement to GTSAM label Aug 20, 2021

johnwlambert reviewed Oct 21, 2021

View reviewed changes

varunagrawal added 2 commits October 21, 2021 15:02

add type annotations

6145466

Merge branch 'develop' into fix/566

86ab7d3

varunagrawal requested a review from johnwlambert October 21, 2021 19:02

johnwlambert reviewed Oct 21, 2021

View reviewed changes

varunagrawal added 2 commits October 21, 2021 15:08

fix type annotation

279c645

use python f-strings

03ac36c

ProfFan approved these changes Oct 21, 2021

View reviewed changes

johnwlambert reviewed Oct 21, 2021

View reviewed changes

python/gtsam/examples/IMUKittiExampleGPS.py Outdated

gps[0], gps[1], gps[2]))

def parse_args():

Copy link

Contributor

johnwlambert Oct 21, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

missing return type here

johnwlambert reviewed Oct 21, 2021

View reviewed changes

python/gtsam/examples/IMUKittiExampleGPS.py Outdated Show resolved Hide resolved

johnwlambert reviewed Oct 21, 2021

View reviewed changes

python/gtsam/examples/IMUKittiExampleGPS.py Show resolved Hide resolved

johnwlambert reviewed Oct 21, 2021

View reviewed changes

python/gtsam/examples/IMUKittiExampleGPS.py Show resolved Hide resolved

johnwlambert reviewed Oct 21, 2021

View reviewed changes

refactor the example to make it cleaner

1e84fd9

varunagrawal requested a review from johnwlambert October 21, 2021 20:33

johnwlambert reviewed Oct 21, 2021

View reviewed changes

replace static variable with variable of greater scope in cpp example

9aa0dbf

fix the included_imu_measurement_count scope

b3e8bf2

johnwlambert approved these changes Oct 21, 2021

View reviewed changes

varunagrawal merged commit 791c7bd into develop Oct 22, 2021

varunagrawal deleted the fix/566 branch October 22, 2021 12:11

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

IMUKittiExampleGPS Python Example #836

IMUKittiExampleGPS Python Example #836

varunagrawal commented Aug 1, 2021

varunagrawal commented Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

ProfFan left a comment

johnwlambert Oct 21, 2021

johnwlambert Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

johnwlambert Oct 21, 2021

varunagrawal Oct 21, 2021

varunagrawal Oct 21, 2021

varunagrawal commented Oct 22, 2021

cxwoaiwojiaa commented Nov 9, 2021

		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):



		class ImuMeasurement:
		"""An instance of an IMU measurement."""

IMUKittiExampleGPS Python Example #836

IMUKittiExampleGPS Python Example #836

Conversation

varunagrawal commented Aug 1, 2021

varunagrawal commented Oct 21, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ProfFan left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

varunagrawal commented Oct 22, 2021

cxwoaiwojiaa commented Nov 9, 2021