Fix the code for imu+mocap calibration.

aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_mocap

@@ -228,7 +228,7 @@ def main():
 
     print("Exporting poses...")
     posesFile = bagtag + "-poses-imumocap-imu0.csv"
-    util.exportPoses(iCal, filename=posesFile)
+    mocap_util.exportPosesHamilton(iCal, filename=posesFile)
     print("  Poses written to {0}".format(posesFile))
     # posesFile = bagtag + "-poses-imumocap-mocap0.csv"
     # mocap_util.exportPoses(iCal, filename=posesFile)

aslam_offline_calibration/kalibr/python/kalibr_common/MocapDatasetReader.py

@@ -121,7 +121,7 @@ def getMessage(self, idx):
             timestamp = acv.Time(self.timestamp_corrector.getLocalTime(data.header.stamp.to_sec()))
         else:
             timestamp = acv.Time(data.header.stamp.secs, data.header.stamp.nsecs)
-        quat = np.array([data.transform.rotation.x, data.transform.rotation.y, data.transform.rotation.z, data.transform.rotation.w])
-        pos = np.array([data.transform.translation.x, data.transform.translation.y, data.transform.translation.z])
+        quat = np.array([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w])
+        pos = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z])
 
         return (timestamp, quat, pos)

aslam_offline_calibration/kalibr/python/kalibr_imu_mocap_calibration/MocapSensors.py

@@ -59,6 +59,8 @@ def __init__(self, parsed):
 
     class MocapMeasurement(object):
         def __init__(self, stamp, quat, pos):
+            # There is a required conjugation between Hamilton's quaternion and JPL convention.
+            quat[:3] = -quat[:3]
             self.T_w_m = sm.Transformation(quat, pos)
             self.stamp = stamp
 

aslam_offline_calibration/kalibr/python/kalibr_imu_mocap_calibration/MocapUtil.py

@@ -259,7 +259,7 @@
 #     if showOnScreen:
 #         plotter.show()
 
-def exportPoses(cself, filename="poses_imu0.csv"):
+def exportPosesHamilton(cself, filename="poses_imu0.csv"):
     # Append our header, and select times at IMU rate
     f = open(filename, 'w')
     print("#timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []", file=f)
@@ -272,12 +272,12 @@ def exportPoses(cself, filename="poses_imu0.csv"):
     # Times are in nanoseconds
     # ETH groundtruth csv format [t,q,p]
     for time in times:
-        T_w_b = sm.Transformation(sm.rt2Transform(bodyspline.orientation(time), bodyspline.position(time)))
-        T_b_m = cself.Mocap.getResultTrafoImuToMocap().inverse()
-        T_m_w = T_w_b * T_b_m
-        time_mocap = time - cself.Mocap.getResultTimeShift()
-        print("{:.0f},".format(1e9 * time_mocap) + ",".join(map("{:.6f}".format, T_m_w.t()))
-              + ",{:.6f},".format(T_m_w.q()[3]) + ",".join(map("{:.6f}".format, T_m_w.q()[0:3])), file=f)
+        position = bodyspline.position(time)
+        orientation = sm.r2quat(bodyspline.orientation(time))
+        # There is a required conjugation between Hamilton's quaternion and JPL convention.
+        orientation[:3] = -orientation[:3]
+        print("{:.0f},".format(1e9 * time) + ",".join(map("{:.6f}".format, position)) \
+               + ",{:.6f},".format(orientation[3]) + ",".join(map("{:.6f}".format, orientation[0:3])) , file=f)
 
 # def saveResultTxt(cself, filename='cam_imu_result.txt'):
 #     f = open(filename, 'w')