Transform problem when using kitti dataset

[zheng rong]

Hi all,

I am trying to process kitti datasets with google cartographer, but I got the error:

[ INFO] [1513667089.796255202]: I1219 15:04:49.000000 22555 submaps.cc:295] Added submap 1
[ INFO] [1513667089.796304461]: I1219 15:04:49.000000 22555 map_builder_bridge.cc:86] Added trajectory with ID '0'.
F1219 15:04:50.196651 22555 sensor_bridge.cc:95] Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise.
[FATAL] [1513667090.196894902, 1513590908.702413002]: F1219 15:04:50.000000 22555 sensor_bridge.cc:95] Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise.
*** Check failure stack trace: ***
    @     0x7f540e1275cd  google::LogMessage::Fail()
    @     0x7f540e129433  google::LogMessage::SendToLog()
    @     0x7f540e12715b  google::LogMessage::Flush()
    @     0x7f540e129e1e  google::LogMessageFatal::~LogMessageFatal()
    @           0x598b02  cartographer_ros::SensorBridge::ToImuData()
    @           0x573450  cartographer_ros::Node::HandleImuMessage()
    @           0x582e7e  boost::detail::function::void_function_obj_invoker1<>::invoke()
    @           0x58be59  ros::SubscriptionCallbackHelperT<>::call()
    @     0x7f540bd9544d  ros::SubscriptionQueue::call()
    @     0x7f540bd3b210  ros::CallbackQueue::callOneCB()
    @     0x7f540bd3c683  ros::CallbackQueue::callAvailable()
    @     0x7f540bd99511  ros::SingleThreadedSpinner::spin()
    @     0x7f540bd7e4cb  ros::spin()
    @           0x56e0e0  cartographer_ros::(anonymous namespace)::Run()
    @           0x56b6b4  main
    @     0x7f540a78b830  __libc_start_main
    @           0x56dc99  _start
    @              (nil)  (unknown)

The TF information from the kitti datasets is:

-
header:
  seq: 0
  stamp:
    secs: 1317021528
    nsecs: 236146926
  frame_id: "base_link"
child_frame_id: "imu_link"
transform:
  translation:
    x: -1.405
    y: 0.32
    z: 0.93
  rotation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
-
header:
  seq: 0
  stamp:
    secs: 1317021528
    nsecs: 236146926
  frame_id: "imu_link"
child_frame_id: "velo_link"
transform:
  translation:
    x: 0.810543903486
    y: -0.307054359206
    z: 0.802724058277
  rotation:
    x: -0.00741208800464
    y: 0.00101508648626
    z: 0.000385188259497
    w: 0.999971937106

I thought I need to modify the frame transform in backpack_3d.urdf.

Saying:

transform T1 of imu_link w.r.t base_link is [quat1, t1], 

transform T2 of velo_link w.r.t imu_link is [quat2, t2], so 

quat1 = [0,0,0,1] (xyzw)
t1 = [-1.405, 0.32, 0.93]

quat2 = [-0.0074, 0.001015, 0.000385, 0.99997] (xyzw)
t2 = [0.8105, -0.307, 0.8027]

So I thought the T3 of velo_link w.r.t base_link (needed in urdf file) should be 

[quat3,t3]=[quat1, t1]*[quat2, t2]

then,

quat3 = [-0.0074, 0.001015, 0.000385, 0.99997](xyzw), corresponding rpy3=[-0.0148, 0.002, 0.000755]
t3=[-0.5945, 0.013, 1.733]

I modified the backpack_3d.urdf like this:

<joint name="imu_link_joint" type="fixed">
 <parent link="base_link" />
 <child link="imu_link" />
 <origin xyz="-1.405, 0.32, 0.93" rpy="0 0 0" />
</joint>


<joint name="horizontal_vlp16_link_joint" type="fixed">
 <parent link="base_link" />
 <child link="horizontal_vlp16_link" />
 <origin xyz="-0.5945, 0.013, 1.733" rpy="-0.0148 0.002 0.0007553" />
</joint>

As cartographer says "The IMU frame must be colocated with the tracking frame" (tracking frame is base_frame), 

I remove the translation offset between imu_link and base_link, so the transforms becomes this:

<joint name="imu_link_joint" type="fixed">
 <parent link="base_link" />
 <child link="imu_link" />
 <origin xyz="0 0 0" rpy="0 0 0" />
</joint>


<joint name="horizontal_vlp16_link_joint" type="fixed">
 <parent link="base_link" />
 <child link="horizontal_vlp16_link" />
 <origin xyz="0.8105 -0.307 0.803" rpy="-0.0148 0.002 0.0007553" />
</joint>

But when I run the code, I still got the same error.

My questions are:

1. At the very beginning, the base-imu translation is [0,0,0], why does the code believe the translation is not zero (The IMU frame must be colocated with the tracking frame)?

2. Is there anything else I missed to run Cartographer properly with general datasets? what is the solution to my problem?

Thanks in advance,

Zheng