Merge pull request ethz-asl#582 from ethz-asl/imu_rate_viz

IMU Rate Visualization

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.

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

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.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+")")
+    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])
 

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)