Added the euroc drone slam tutorial

README.md

@@ -8,6 +8,9 @@ Below is a list of all tutorials available in this repository:
 ### [Foxglove Visualization for 1X World Model Challenge](datasets/1x_eve/README.md)
 - 📝 Use Foxglove to visualize 1X World Model Challenge
 - 🎥 [Video](https://www.youtube.com/watch?v=A9wl491ltPA)
+### [EuRoC MAV Dataset in Foxglove](datasets/EuRoC_MAV/README.md)
+- 📝 Drone SLAM of the MH01 Environment from EuRoC MAV
+- 🔗 [Related Blog Post](https://foxglove.dev/blog/converting-euroc-mav-dataset-to-mcap)
 ### [Convert .mat files to MCAP](datasets/NASA_mat_to_MCAP/README.md)
 - 📝 Visualize Matlab files for NASA Valkyrie box pickup task
 ### [Converting Argoverse 2 Dataset to MCAP](datasets/foxglove_av2_tutorial/README.md)

datasets/EuRoC_MAV/.gitignore

@@ -0,0 +1,3 @@
+*.mcap
+data/*
+.env

datasets/EuRoC_MAV/README.md

@@ -0,0 +1,45 @@
+---
+title: "EuRoC MAV Dataset in Foxglove"
+short_description: "Drone SLAM of the MH01 Environment from EuRoC MAV"
+blog_post_url: "https://foxglove.dev/blog/converting-euroc-mav-dataset-to-mcap"
+---
+
+# EuRoC MAV Dataset in Foxglove
+
+This tutorial shows how to convert the EuRoC MAV dataset into an MCAP file for visualization in Foxglove.
+
+![SLAM_gif](media/MH01_Final_gif_cut.gif)
+
+## Run this tutorial locally:
+1. Download one of the EuRoC environments from [ETH Zurich](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) in ASL format. The Machine Hall 01 set is visualized in the tutorial.
+2. Make sure to have a working [ROS distribution](https://docs.ros.org/en/humble/Installation.html) and install dependancies:
+```bash
+sudo apt install \
+   ros-$ROS_DISTRO-foxglove-bridge \
+   ros-$ROS_DISTRO-imu-tools \
+   ros-$ROS_DISTRO-rtabmap-ros \
+   ros-$ROS_DISTRO-rosbag2-storage-mcap \
+   ros-$ROS_DISTRO-image-proc
+```
+In a new python environment, install:
+```bash
+pip install foxglove-sdk numpy opencv-python pyyaml
+```
+3. Convert the dataset to MCAP
+```bash
+python3 convert-euroc-2-mcap.py --src <dataset_dir> --dst <mcap_output_location>.mcap
+```
+4. Build the ROS 2 package called euroc_slam
+```bash
+cd ~/tutorials/EuRoC_MAV/ros2_ws
+colcon build --symlink-install
+source install/setup.bash
+```
+5. Run SLAM and visualize in Foxglove while the dataset plays back:
+```bash
+ros2 launch euroc_slam firefly.launch.py
+```
+In a seperate terminal:
+```bash
+ros2 bag play <mcap_file> --clock
+```

datasets/EuRoC_MAV/convert-euroc-2-mcap.py

@@ -0,0 +1,180 @@
+import foxglove
+import argparse
+import os
+import csv
+import cv2
+import yaml
+import numpy as np
+from foxglove import Schema, Channel
+from sensor_msgs.msg import Image, Imu
+from std_msgs.msg import Header
+from geometry_msgs.msg import Quaternion, Vector3
+from rclpy.serialization import serialize_message
+from pathlib import Path
+
+
+def get_image_msg(img_path: str, timestamp: int, cam_num: int) -> Image:
+    # Stereo images should be loaded in as grayscale
+    img = cv2.imread(img_path, cv2.IMREAD_GRAYSCALE)
+    if img is None:
+        raise ValueError(f"Could not load image: {img_path}")
+
+    height, width = img.shape
+
+    sec = int(timestamp // 1e9)
+    nsec = int(timestamp % 1e9)
+
+    # Populate the image message data
+    ros_image = Image()
+    ros_image.header = Header()
+    ros_image.header.stamp.sec = sec
+    ros_image.header.stamp.nanosec = nsec
+    ros_image.header.frame_id = f"cam{cam_num}"
+    ros_image.height = height
+    ros_image.width = width
+    ros_image.encoding = "mono8"  # Stereo images
+    ros_image.step = width  # For mono8, 1 byte per pixel
+    ros_image.data = img.tobytes()
+
+    return ros_image
+
+
+def read_images(cam_directory: str, channel: Channel, cam_num: int) -> None:
+    # Loop through the data.csv file and read in the image files
+    csv_path = os.path.join(cam_directory, "data.csv")
+    with open(csv_path, "r") as csv_file:
+        reader = csv.reader(csv_file)
+        next(reader)  # Skip the first row with headers
+        for row in reader:
+            timestamp = int(row[0])
+            image_name = row[1]
+            image_path = os.path.join(cam_directory, "data", image_name)
+            if not os.path.exists(image_path):
+                print(f"Image {image_path} does not exist")
+                continue
+
+            # Convert image to ROS2 message and write to channel
+            image_msg = get_image_msg(image_path, timestamp, cam_num)
+            channel.log(serialize_message(image_msg), log_time=timestamp)
+
+
+def read_imu(imu_data_path: str, imu_channel: Channel, imu_yaml_path: str) -> None:
+    """Read IMU data and write to MCAP channel.
+
+    IMU data is in the format:
+    timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]
+
+    We will convert this to a custom IMU message that interfaces with sensor_msgs/msgs/Imu.
+    This will be in the form:
+     - header (header)
+     - orientation (quaternion)
+     - orientation covariance (float32[9])
+     - angular velocity (vector3)
+     - angular velocity covariance (float32[9])
+     - linear acceleration (vector3)
+     - linear acceleration covariance (float32[9])
+
+    Args:
+        imu_data_path: Path to IMU data CSV file
+        imu_channel: MCAP channel to write IMU messages to
+        imu_yaml_path: Path to IMU configuration YAML file
+    """
+    # Get the IMU config with covariance information
+    with open(imu_yaml_path, "r") as imu_yaml_file:
+        imu_yaml = yaml.load(imu_yaml_file, Loader=yaml.FullLoader)
+
+    # Get the noise and bias parameters, see https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model for more details
+    sample_sqr_dt = np.sqrt(1.0 / float(imu_yaml["rate_hz"]))
+    sigma_gd = imu_yaml["gyroscope_noise_density"] * sample_sqr_dt
+    sigma_ad = imu_yaml["accelerometer_noise_density"] * sample_sqr_dt
+
+    # Calculate the covariance matrices
+    orientation_cov = np.zeros((3, 3), dtype=np.float64)
+    angular_velocity_cov = np.diag([sigma_gd**2, sigma_gd**2, sigma_gd**2]).astype(np.float64)
+    linear_acceleration_cov = np.diag([sigma_ad**2, sigma_ad**2, sigma_ad**2]).astype(np.float64)
+
+    with open(imu_data_path, "r") as imu_file:
+        reader = csv.reader(imu_file)
+        next(reader)  # Skip the first row with headers
+        for row in reader:
+            timestamp = int(row[0])
+            angular_velocity = [float(row[1]), float(row[2]), float(row[3])]
+            linear_acceleration = [float(row[4]), float(row[5]), float(row[6])]
+
+            imu_msg = Imu()
+            imu_msg.header = Header()
+            imu_msg.header.stamp.sec = timestamp // int(1e9)
+            imu_msg.header.stamp.nanosec = timestamp % int(1e9)
+            imu_msg.header.frame_id = "imu4"  # Transformation reference frame
+            # Orientation
+            imu_msg.orientation = Quaternion()
+            imu_msg.orientation.x = 0.0
+            imu_msg.orientation.y = 0.0
+            imu_msg.orientation.z = 0.0
+            imu_msg.orientation.w = 1.0
+            imu_msg.orientation_covariance = list(orientation_cov.flatten(order="C").astype(float))
+            # Angular velocity
+            imu_msg.angular_velocity = Vector3()
+            imu_msg.angular_velocity.x = angular_velocity[0]
+            imu_msg.angular_velocity.y = angular_velocity[1]
+            imu_msg.angular_velocity.z = angular_velocity[2]
+            imu_msg.angular_velocity_covariance = list(angular_velocity_cov.flatten(order="C").astype(float))
+            # Linear acceleration
+            imu_msg.linear_acceleration = Vector3()
+            imu_msg.linear_acceleration.x = linear_acceleration[0]
+            imu_msg.linear_acceleration.y = linear_acceleration[1]
+            imu_msg.linear_acceleration.z = linear_acceleration[2]
+            imu_msg.linear_acceleration_covariance = list(linear_acceleration_cov.flatten(order="C").astype(float))
+
+            imu_channel.log(serialize_message(imu_msg), log_time=timestamp)
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description='Convert Euroc dataset to MCAP format')
+    parser.add_argument('--src', type=str, required=True, help='Path to the Euroc dataset directory')
+    parser.add_argument('--dst', type=str, required=True, help='Path to the output MCAP file')
+    args = parser.parse_args()
+
+    out_mcap_path = args.dst
+    if os.path.exists(out_mcap_path):  # Remove the previous file if it already exists
+        os.remove(out_mcap_path)
+
+    writer = foxglove.open_mcap(out_mcap_path, allow_overwrite=True)  # Open the mcap file for writing
+
+    imu_msg_path = Path("msgs/imu_flat.msg")
+    img_msg_path = Path("msgs/image_flat.msg")
+
+    img_schema = Schema(
+        name="sensor_msgs/msg/Image",
+        encoding="ros2msg",
+        data=img_msg_path.read_bytes(),
+    )
+    imu_schema = Schema(
+        name="sensor_msgs/msg/Imu",
+        encoding="ros2msg",
+        data=imu_msg_path.read_bytes(),
+    )
+
+    # ros2msg channels require cdr encoding type
+    cam0_channel = Channel(topic="/cam0/image_raw", schema=img_schema, message_encoding="cdr")
+    cam1_channel = Channel(topic="/cam1/image_raw", schema=img_schema, message_encoding="cdr")
+    imu_channel = Channel(topic="/imu0", schema=imu_schema, message_encoding="cdr")
+
+    # Read in the data.csv file
+    data_csv_path = os.path.join(args.src, "data.csv")
+    cam0_dir = os.path.join(args.src, "cam0")
+    cam1_dir = os.path.join(args.src, "cam1")
+    imu_dir = os.path.join(args.src, "imu0", "data.csv")
+    imu_yaml_path = os.path.join(args.src, "imu0", "sensor.yaml")
+
+    read_images(cam0_dir, cam0_channel, 0)
+    print("Done writing cam0")
+    read_images(cam1_dir, cam1_channel, 1)
+    print("Done writing cam1")
+    read_imu(imu_dir, imu_channel, imu_yaml_path)
+    print("Done writing imu")
+
+
+if __name__ == "__main__":
+    main()
+

datasets/EuRoC_MAV/data/README.md

@@ -0,0 +1,3 @@
+Save your Euroc format datasets here!
+
+See https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets for examples.