[TODOs] About deal with tracklets

[harderthan]

Hello, maintainers. @tomas789 @emreay- @jnitsch .
I'm interested in contribution to develop "deal with tracklets" as a feature request.
I'm going to make a Pull-Request for that.
So, I check the Kitti Tracklet file and I think it is good to use this util from here

We can use the module as parser like following codes.

from parseTrackletXML import Tracklet,parseXML

tracklets = parseXML('data/2011_09_26/2011_09_26_drive_0002_sync/tracklet_labels.xml')

After this parsing progress, what kind of the type we need for kitti2bag ?
How about just making a custom topic message for item of tracklet_labels.xml?

If you have a good other way, please recommend it.

[thully]

I forked the project and added support for tracklets using parseTrackletXML. For the tracklet data message I used the Marker message type, and a MarkerArray to contain all the markers for a given frame. This seems to work for my own purposes, though it may need to be revised a bit if you want to use the Markers in rviz as-is (I'm not using rviz so didn't test there). Also, some of the tracklet data isn't transferred to the Markers, and some of the fields I used for data may not fit their typical use in other applications.

https://github.com/thully/kitti2bag

[valgur]

@thully That's really nice. We should definitely consider merging this into the main codebase. I'm only wondering if it could perhaps be based off pykitti like the rest of the code is? There is a tracklet class in pykitti that might be useful here. This would also avoid using parseTrackletXML.py which is slightly problematic due to it lacking explicit licensing info.

[thully]

The pykitti tracklet class seems to be designed to read a different set of tracking data, not the tracklet xml that parseTrackletXML does.

[epicjung]

from os.path import join    
from parseTrackletXML import parseXML

def save_tracklet_data(bag, kitti, velo_frame_id, topic):
    
   print("Exporting tracklets")

    # read tracklets from file
    tracklet_file = join(kitti.data_path, 'tracklet_labels.xml')
    tracklets = parseXML(tracklet_file)

    # loop over tracklets
    for time_idx, timestamp in enumerate(kitti.timestamps):
        bbox_array_msg = BoundingBoxArray()
        bbox_array_msg.header.frame_id = velo_frame_id
        bbox_array_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        for i, tracklet in enumerate(tracklets):
            h, w, l = tracklet.size
            for translation, rotation, state, occlusion, truncation, amt_occlusion, amt_borders, frame_num in tracklet:
                if frame_num == time_idx:
                    bbox = BoundingBox()
                    bbox.header.frame_id = velo_frame_id
                    bbox.header.stamp = bbox_array_msg.header.stamp
                    bbox.dimensions.x = l
                    bbox.dimensions.y = w
                    bbox.dimensions.z = h
                    bbox.label = i
                    bbox.pose.position.x = translation[0]
                    bbox.pose.position.y = translation[1]
                    bbox.pose.position.z = translation[2] + h/2.0
                    yaw = rotation[2]
                    q = quaternion_from_euler(0, 0, yaw)
                    bbox.pose.orientation.x = q[0]
                    bbox.pose.orientation.y = q[1]
                    bbox.pose.orientation.z = q[2]
                    bbox.pose.orientation.w = q[3]
                    assert np.abs(rotation[:2]).sum() == 0, 'object roatations other than yaw given!'
                    # print("{}: label: {}, {} {} {}".format(frame_num, i, translation[0], translation[1], translation[2]))
                    bbox_array_msg.boxes.append(bbox)
        # print("{}: bbox num: {}".format(float(timestamp.strftime("%s.%f")), len(bbox_array_msg.boxes)))
        bag.write(topic, bbox_array_msg, t=bbox_array_msg.header.stamp)

by using parseTrackletXML.py in https://github.com/thully/kitti2bag and above code, you can record the bounding boxes into a bag file.