Skip to content

Commit

Permalink
Merge pull request #5 from dom-lee/traj_eval
Browse files Browse the repository at this point in the history
Added eval script and edited readme
andrewwwei authored Apr 20, 2023
2 parents e3d0b17 + 448f83d commit d7b8c09
Showing 2 changed files with 59 additions and 0 deletions.
8 changes: 8 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -32,3 +32,11 @@ roslaunch multiple-object-tracking kitti-raw.launch /
```
rosbag play <path_to_bag_file>
```

## Evaluate Trajectories
Use this script to combine bagfiles from LIO-SAM and Masked LIO-SAM outputs for plotting them together using EVO (https://github.com/MichaelGrupp/evo).
Edit `scene` inside `eval/traj_eval.py` to the corresponding KITTI scene, then run:
```
cd eval/
python traj_eval.py
```
51 changes: 51 additions & 0 deletions eval/traj_eval.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
import os
from rosbags.rosbag1 import Reader
from rosbags.rosbag1 import Writer
from rosbags.serde import deserialize_cdr, ros1_to_cdr, cdr_to_ros1, serialize_cdr

scene = '0047'

combined_file = 'traj_eval/combined_lio-sam_' + scene + '_traj_eval_data.bag'
lio_file = 'traj_eval/lio-sam_' + scene + '_traj_eval_data.bag'
masked_file = 'traj_eval/masked_lio-sam_' + scene + '_traj_eval_data.bag'

# topics = ['/odometry/gps', '/odometry/imu_incremental', '/lio_sam/mapping/odometry_incremental']
msg_dict = {}
connection_dict = {}

if os.path.exists(combined_file):
os.remove(combined_file)

# create reader instance
with Writer(combined_file) as writer:
with Reader(lio_file) as reader_lio:
for connection, timestamp, rawdata in reader_lio.messages():
# if connection.topic in topics:
topic = connection.topic

if topic not in msg_dict.keys():
msg_dict[topic] = []
connection_dict[topic] = writer.add_connection(topic, connection.msgtype, latching=1)

msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype), connection.msgtype)
write_msg = cdr_to_ros1(serialize_cdr(msg, connection.msgtype), connection.msgtype)
msg_dict[topic].append((connection_dict[topic], timestamp, write_msg))


with Reader(masked_file) as reader_masked:
for connection, timestamp, rawdata in reader_masked.messages():
# if connection.topic in topics:
topic = '{}_masked'.format(connection.topic)

if topic not in msg_dict.keys():
msg_dict[topic] = []
connection_dict[topic] = writer.add_connection(topic, connection.msgtype, latching=1)

msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype), connection.msgtype)
write_msg = cdr_to_ros1(serialize_cdr(msg, connection.msgtype), connection.msgtype)
msg_dict[topic].append((connection_dict[topic], timestamp, write_msg))

for write_topics in msg_dict.values():
for tup in write_topics:
write_connection, timestamp, write_msg = tup
writer.write(write_connection, timestamp, write_msg)

0 comments on commit d7b8c09

Please sign in to comment.