From 6de14bc89823e3490f6109f0f4c836a18ca28f0a Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 24 Nov 2022 17:55:28 -0500 Subject: [PATCH 1/3] add plot of imu rate to debug bad IMUs with irregular rates --- .../kalibr_camera_calibration/CameraUtils.py | 3 +- .../kalibr_imu_camera_calibration/IccPlots.py | 51 +++++++++++++++++++ .../kalibr_imu_camera_calibration/IccUtil.py | 10 ++++ 3 files changed, 63 insertions(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py index 1fc12fb3f..5c9de6cd9 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py @@ -26,7 +26,8 @@ import gc import sys -np.set_printoptions(suppress=True, precision=8) +# make numpy print prettier +np.set_printoptions(suppress=True) def normalize(v): diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py index c8e463957..1006c9b49 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py @@ -1,6 +1,57 @@ import numpy as np import pylab as pl + +def plotIMURates(cself, iidx, fno=1, clearFigure=True, noShow=False): + #timestamps we have me + imu = cself.ImuList[iidx] + bodyspline = cself.poseDv.spline() + timestamps = np.array([im.stamp.toSec() + imu.timeOffset for im in imu.imuData \ + if im.stamp.toSec() + imu.timeOffset > bodyspline.t_min() \ + and im.stamp.toSec() + imu.timeOffset < bodyspline.t_max() ]) + + + scale = 1000.0 + unit = "ms" + z_thresh = 1.2 + + #calculate the relative rate between readings + times = [] + rates = [] + for idx in range(1,len(timestamps)): + times.append(timestamps[idx] - timestamps[0]) + rates.append(scale * (timestamps[idx] - timestamps[idx-1])) + rate_avg = np.average(rates) + rate_std = np.std(rates) + + #loop through and so z-test to find outliers + #https://en.wikipedia.org/wiki/Z-test + sizes = [] + colors = [] + for idx in range(0,len(rates)): + rate = rates[idx] + #if (abs(rate - rate_avg)/rate_std) > z_thresh: + # sizes.append(5) + # colors.append("r") + #else: + sizes.append(1) + colors.append("b") + # rates[idx] = abs(rate - rate_avg) + rates[idx] = rate + + #plot it + f = pl.figure(fno) + if clearFigure: + f.clf() + f.suptitle("imu{0}: sample inertial rate".format(iidx)) + pl.scatter(times, rates, s=sizes, c=colors, marker="x") + pl.text(0.1, 0.2, 'avg dt ('+unit+') = {:.2f} +- {:.4f}'.format(rate_avg, rate_std), fontsize=12, transform=f.gca().transAxes) + pl.grid('on') + pl.xlabel("time (s)") + pl.ylabel("sample rate ("+unit+")") + f.gca().set_xlim((min(times), max(times))) + f.gca().set_ylim((0.0, max(rates))) + def plotGyroError(cself, iidx, fno=1, clearFigure=True, noShow=False): errors = np.array([np.dot(re.error(), re.error()) for re in cself.ImuList[iidx].gyroErrors]) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py index 020ec1f0c..b07888094 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccUtil.py @@ -19,6 +19,9 @@ from io import StringIO import matplotlib.patches as patches +# make numpy print prettier +np.set_printoptions(suppress=True) + def plotTrajectory(cself, fno=1, clearFigure=True, title=""): f = pl.figure(fno) @@ -195,6 +198,13 @@ def generateReport(cself, filename="report.pdf", showOnScreen=True): #plot imu stuff (if we have imus) for iidx, imu in enumerate(cself.ImuList): + + f = pl.figure(offset+iidx) + plots.plotIMURates(cself, iidx, fno=f.number, noShow=True) + plotter.add_figure("imu{0}: measurement rates".format(iidx), f) + figs.append(f) + offset += len(cself.ImuList) + f = pl.figure(offset+iidx) plots.plotAccelerations(cself, iidx, fno=f.number, noShow=True) plotter.add_figure("imu{0}: accelerations".format(iidx), f) From d2cfd323df05056347ce3168613f6261dde33f34 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 24 Nov 2022 18:53:29 -0500 Subject: [PATCH 2/3] display in top left --- .../kalibr/python/kalibr_imu_camera_calibration/IccPlots.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py index 1006c9b49..893db3252 100644 --- a/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py +++ b/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccPlots.py @@ -45,7 +45,7 @@ def plotIMURates(cself, iidx, fno=1, clearFigure=True, noShow=False): f.clf() f.suptitle("imu{0}: sample inertial rate".format(iidx)) pl.scatter(times, rates, s=sizes, c=colors, marker="x") - pl.text(0.1, 0.2, 'avg dt ('+unit+') = {:.2f} +- {:.4f}'.format(rate_avg, rate_std), fontsize=12, transform=f.gca().transAxes) + pl.text(0.1, 0.9, 'avg dt ('+unit+') = {:.2f} +- {:.4f}'.format(rate_avg, rate_std), fontsize=12, transform=f.gca().transAxes) pl.grid('on') pl.xlabel("time (s)") pl.ylabel("sample rate ("+unit+")") From d6ddcbec7cd9acbe9c611038c022ee79003b346d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 24 Nov 2022 18:53:39 -0500 Subject: [PATCH 3/3] update readme [skip ci] --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 7855e736a..a718e1252 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,7 @@ For questions or comments, please open an issue on Github. ## News / Events +* **Nov 24, 2022** - Some new visualization of trajectory and IMU rate for the generated report along with fixed support for exporting poses to file (see PR [#578](https://github.com/ethz-asl/kalibr/pull/578),[#581](https://github.com/ethz-asl/kalibr/pull/581),[#582](https://github.com/ethz-asl/kalibr/pull/582)) * **May 3, 2022** - Support for Ubuntu 20.04 along with Docker scripts have been merged into master via PR [#515](https://github.com/ethz-asl/kalibr/pull/515). A large portion was upgrading to Python 3. A special thanks to all the contributors that made this possible. Additionally, contributed fixes for the different validation and visualization scripts have been merged. * **Febuary 3, 2020** - Initial Ubuntu 18.04 support has been merged via PR [#241](https://github.com/ethz-asl/kalibr/pull/241). Additionally, support for inputting an initial guess for focal length can be provided from the cmd-line on failure to initialize them. * **August 15, 2018** - Double sphere camera models have been contributed to the repository via PR [#210](https://github.com/ethz-asl/kalibr/pull/210). If you are interested you can refer to the [paper](https://arxiv.org/abs/1807.08957) for a nice overview of the models in the repository.