New Feature: Continual SLAM (#424)

* initial commit * feat(Continual-SLAM): Finished __getitem__ dataset iterator Currently, still work in progress. But I have finished the simple data iteration, which returns the OpenDR type Image, Distance and Speed * feat(Continual-SLAM): Wrote the ROS node for image, velocity and distance publisher Currently still work in progress. Wrote the ROS node which publishes the sequantial images from KITTI as well as it publishes the velocity and distances at that current moment respectively * feat(Continual-SLAM): Extended ROS node to hold past 3 data values * feat(Continual-SLAM): [WIP] Implementation of CL-SLAM expert inference * feat(Continual-SLAM): [WIP] Implementation of CL-SLAM expert inference cont. started implementation of inference * feat(Continual-SLAM): Reverted dataset publisher into original single mode The caching of the previous times and previous images will be done in the reciever node * feat(Continual-SLAM): [WIP] Implemented predict function for depth-pose networks * feat(Continual-SLAM): [WIP] Started implementation of Learner class together with ROS Node (just for prediction mode now) * feat(Continual-SLAM): [WIP] ROS node for CL-SLAM Predictor * feat(Continual-SLAM): Add reconstruct depth images from disparity * feat(Continual-SLAM): [WIP] Ros node and learner class * feat(Continual-SLAM): [WIP] ROS nodes are implemented, synch is missing * feat(Continual-SLAM): ROS Node and Learner Implementation for only Predictor is done * feat(Continual-SLAM): [WIP] Fit/Adapt function is implemented, ROS Node publisher coordinate change * fix(Continual-SLAM): Fix small predictor mode requirements * feat(Continual-SLAM): [WIP] Learner/Predictor + ROS Nodes imp. done, need to fix bugs * refactor(Continual-SLAM): Refactored some parts to increase readability * feat(Continual-SLAM): [WIP] Working on adapt function * feat(Continual-SLAM): [WIP] Fix asterisk imports and init files * feat(Continual-SLAM): [WIP] Small addition for debugging, this commit can be ignored later on * refactor(Continual-SLAM): [WIP] changing couple of things to debug, not an important commit * feat(Continual-SLAM): Fixed algorithm, maybe there is room to improve * feat(Continual-SLAM): [WIP] trying to improve dataset publisher and visualization * feat(Continual-SLAM): [WIP] Added multi-device support back * feat(Continual-SLAM): Initial commit to Replay Buffer * feat(Continual-SLAM): [WIP] Replay buffer cont. * feat(Continual-SLAM): [WIP] for Replay buffer implementation * feat(Continual-SLAM): [WIP] Finished replay buffer implementation for ROS nodes as well * refactor(Continual-SLAM): Better namings, improvement in code * refactor(Continual-SLAM): Improvement of code quality for predictor node * refactor(Continual-SLAM): Improve code quality of ROS nodes * refactor(Continual-SLAM): Code quality improvement in CL-SLAM module * feat(Continual-SLAM): [WIP] new implementation of learner * feat(Continual-SLAM): [WIP] Fixed algorithm, improve replay buffer * feat(Continual-SLAM): [WIP] Improved ROS nodes * feat(Continual-SLAM): [WIP] Writing ROS2 nodes * feat(Continual-SLAM): Add ROS2 nodes * feat(Continual-SLAM): Added ROS2 node, and started writing tests * refactor(Continual-SLAM): Update default buffer size and publish rate for learner node * feat(Continual-SLAM): Added Loop Closure Detection * feat(Continual-SLAM): Fix small issues on Loop Closure Detection * feat(Continual-SLAM): [WIP] Loop Closure Performance * feat(Continual-SLAM): [WIP] Loop Closure Detection prob continues * feat(Continual-SLAM): Fixed LoopClosure * refactor(Continual-SLAM): Refactor code fix style problems, add LC update to ROS1 nodes * refactor(Continual-SLAM): Enable fit and infer tests * fix(Continual-SLAM): Fix small comment issue * feat(Continual-SLAM): [WIP] Demo * refactor(Continual-SLAM): Fix styling and get rid of debug lines * feat(Continual-SLAM): Update demo, add README's * docs(Continual-SLAM): Added docs for ROS nodes * feat(Continual-SLAM): Done with unit tests and demo * fix(Continual-SLAM): Fix typo bug on kitti.py * Adapt readme * feat(Continual-SLAM): Add publish pointcloud * feat(Continual-SLAM): Add pointcloud publish for ROS2 node * feat(Continual-SLAM): Add position graph for demo * feat(Continual-SLAM): Add dependencies * docs(Continual-SLAM): Fix deleting previous section mistake * feat(Continual-SLAM): Fix pep8, fix confused dataset files * feat(Continual-SLAM): Fix pep8, add pointcloud screenshot * fix(Continual-SLAM): Fix undetected local pep8 mistakes * feat(Continual-SLAM): Add g2o into skipped dirs for cpp style check * fix(Continual-SLAM): Place the skip dir correctly * fix(Continual-SLAM): Fix clang format * test(Continual-SLAM): Remove Loop Closure test due to not having g2o on test server * feat(Continual-SLAM): Update dependencies * feat(Continual-SLAM): Add the automatic installation of fixed g2opy * Update torch versions * Fix pep8 --------- Co-authored-by: Niclas Vödisch <[email protected]> Co-authored-by: Niclas <[email protected]> Co-authored-by: Nikolaos Passalis <[email protected]>
opendr-eu · Sep 13, 2023 · 108bd9d · 108bd9d
1 parent 6cf2bbf
commit 108bd9d
Show file tree

Hide file tree

Showing 64 changed files with 5,794 additions and 13 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -5,3 +5,6 @@
 	path = src/opendr/perception/panoptic_segmentation/efficient_lps/algorithm/EfficientLPS
 	url = https://github.com/robot-learning-freiburg/EfficientLPS.git
 	branch = opendr
+[submodule "src/opendr/perception/continual_slam/algorithm/g2o/g2opy"]
+	path = src/opendr/perception/continual_slam/algorithm/g2o/g2opy
+	url = https://github.com/uoip/g2opy
diff --git a/projects/opendr_ws/README.md b/projects/opendr_ws/README.md
@@ -90,6 +90,9 @@ Currently, apart from tools, opendr_ws contains the following ROS nodes (categor
 1. [RGBD Hand Gesture Recognition](src/opendr_perception/README.md#rgbd-hand-gesture-recognition-ros-node)
 ## RGB + Audio input
 1. [Audiovisual Emotion Recognition](src/opendr_perception/README.md#audiovisual-emotion-recognition-ros-node)
+
+## RGB + IMU input
+1. [Continual SLAM](src/opendr_perception//README.md#continual-slam-ros-nodes)
 ## Audio input
 1. [Speech Command Recognition](src/opendr_perception/README.md#speech-command-recognition-ros-node)
 2. [Speech Transcription](src/opendr_perception/README.md#speech-transcription-ros-node)

diff --git a/projects/opendr_ws/src/opendr_bridge/src/opendr_bridge/bridge.py b/projects/opendr_ws/src/opendr_bridge/src/opendr_bridge/bridge.py
@@ -21,6 +21,9 @@
 
 import numpy as np
 import struct
+
+import rospy
+from rospy.rostime import Time
 from cv_bridge import CvBridge
 from vision_msgs.msg import Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose,\
      Detection3DArray, Detection3D, BoundingBox3D as BoundingBox3DMsg, ObjectHypothesis, Classification2D
@@ -29,11 +32,17 @@
 from std_msgs.msg import ColorRGBA, String, Header
 from sensor_msgs.msg import Image as ImageMsg, PointCloud as PointCloudMsg, PointCloud2 as PointCloud2Msg,\
      ChannelFloat32 as ChannelFloat32Msg, PointField as PointFieldMsg
-import rospy
-from geometry_msgs.msg import Point32 as Point32Msg, Quaternion as QuaternionMsg
+from geometry_msgs.msg import (
+    Point32 as Point32Msg,
+    Quaternion as QuaternionMsg,
+    Vector3Stamped as Vector3StampedMsg,
+    TransformStamped as TransformStampedMsg
+)
+from visualization_msgs.msg import Marker as MarkerMsg, MarkerArray as MarkerArrayMsg
 from sensor_msgs import point_cloud2 as pc2
 from hri_msgs.msg import LiveSpeech
 from opendr_bridge.msg import OpenDRPose2D, OpenDRPose2DKeypoint
+import tf.transformations as tr
 
 
 class ROSBridge:
@@ -61,18 +70,31 @@ def from_ros_image(self, message: ImageMsg, encoding: str='passthrough') -> Imag
         image = Image(np.asarray(cv_image, dtype=np.uint8))
         return image
 
-    def to_ros_image(self, image: Image, encoding: str='passthrough') -> ImageMsg:
+    def to_ros_image(self,
+                     image: Image,
+                     encoding: str='passthrough',
+                     frame_id: str = None,
+                     time: Time = None) -> ImageMsg:
         """
         Converts an OpenDR image into a ROS image message
         :param image: OpenDR image to be converted
         :type image: engine.data.Image
         :param encoding: encoding to be used for the conversion (inherited from CvBridge)
         :type encoding: str
+        :param frame_id: frame id of the image
+        :type frame_id: str
+        :param time: time of the image
+        :type time: rospy.rostime.Time
         :return: ROS image
         :rtype: sensor_msgs.msg.Image
         """
         # Convert from the OpenDR standard (CHW/RGB) to OpenCV standard (HWC/BGR)
-        message = self._cv_bridge.cv2_to_imgmsg(image.opencv(), encoding=encoding)
+        header = Header()
+        if frame_id is not None:
+            header.frame_id = frame_id
+        if time is not None:
+            header.stamp = time
+        message = self._cv_bridge.cv2_to_imgmsg(image.opencv(), encoding=encoding, header=header)
         return message
 
     def to_ros_pose(self, pose: Pose):
@@ -641,7 +663,7 @@ def from_ros_point_cloud2(self, point_cloud: PointCloud2Msg):
 
         return result
 
-    def to_ros_point_cloud2(self, point_cloud: PointCloud, channels: str = None):
+    def to_ros_point_cloud2(self, point_cloud: PointCloud, channels: str = None, frame_id="base_link"):
 
         """
         Converts an OpenDR PointCloud message into a ROS PointCloud2
@@ -655,7 +677,7 @@ def to_ros_point_cloud2(self, point_cloud: PointCloud, channels: str = None):
 
         header = Header()
         header.stamp = rospy.Time.now()
-        header.frame_id = "base_link"
+        header.frame_id = frame_id
 
         channel_count = point_cloud.data.shape[-1] - 3
 
@@ -760,6 +782,164 @@ def to_ros_boxes_3d(self, boxes_3d: BoundingBox3DList, classes):
             ros_boxes_3d.detections.append(box)
         return ros_boxes_3d
 
+    def to_ros_marker(self,
+                      frame_id: str,
+                      position: list,
+                      id: int,
+                      rgba: tuple = None) -> MarkerMsg:
+        """
+        Creates ROS Marker message given positions x,y,z and frame_id.
+        :param frame_id: The frame_id of the marker.
+        :type frame_id: str
+        :param position: The position of the marker.
+        :type position: list
+        :param id: The id of the marker.
+        :type id: int
+        :param rgba: The color of the marker.
+        :type rgba: tuple
+        :return: ROS Marker message.
+        :rtype: visualization_msgs.msg.Marker
+        """
+        marker = MarkerMsg()
+        marker.header.frame_id = frame_id
+        marker.header.stamp = rospy.Time.now()
+        marker.type = marker.SPHERE
+        marker.id = id
+        marker.action = marker.ADD
+        marker.pose.position.x = position[0]
+        marker.pose.position.y = position[1]
+        marker.pose.position.z = position[2]
+        marker.pose.orientation.x = 0.0
+        marker.pose.orientation.y = 0.0
+        marker.pose.orientation.z = 0.0
+        marker.pose.orientation.w = 1.0
+        marker.scale.x = 1.0
+        marker.scale.y = 1.0
+        marker.scale.z = 1.0
+        if rgba is not None:
+            marker.color.a = rgba[3]
+            marker.color.r = rgba[0] / 255.0
+            marker.color.g = rgba[1] / 255.0
+            marker.color.b = rgba[2] / 255.0
+        else:
+            marker.color.a = 1.0
+            marker.color.r = 1.0
+            marker.color.g = 0.0
+            marker.color.b = 0.0
+        return marker
+
+    def to_ros_marker_array(self,
+                            position_list: list,
+                            frame_id_list: list,
+                            rgba: tuple = None) -> MarkerArrayMsg:
+        """
+        Creates ROS MarkerArray message given positions x,y,z and frame_id.
+        :param position_list: The list of positions of the markers.
+        :type position_list: list
+        :param frame_id_list: The list of frame_ids of the markers.
+        :type frame_id_list: list
+        :param rgba: The color of the marker.
+        :type rgba: tuple
+        """
+        marker_array = MarkerArrayMsg()
+        for i in range(len(position_list)):
+            marker_array.markers.append(self.to_ros_marker(frame_id_list[i],
+                                                           position_list[i],
+                                                           i,
+                                                           rgba))
+        return marker_array
+
+    def to_ros_vector3_stamped(self,
+                               x: float,
+                               y: float,
+                               z: float,
+                               frame_id: str,
+                               time: Time) -> Vector3StampedMsg:
+        """
+        Creates a Vector3Stamped message given x,y,z coordinates and frame_id and time
+        :param x: The x coordinate of the vector.
+        :type x: float
+        :param y: The y coordinate of the vector.
+        :type y: float
+        :param z: The z coordinate of the vector.
+        :type z: float
+        :param frame_id: The frame_id of the vector.
+        :type frame_id: str
+        :param time: The time of the vector.
+        :type time: rospy.rostime.Time
+        :return: ROS message with the vector.
+        :rtype: geometry_msgs.msg.Vector3Stamped
+        """
+
+        message = Vector3StampedMsg()
+        message.header.frame_id = frame_id
+        message.header.stamp = time
+        message.vector.x = x
+        message.vector.y = y
+        message.vector.z = z
+
+        return message
+
+    def from_ros_vector3_stamped(self, message: Vector3StampedMsg):
+        """
+        Creates a Vector3Stamped message given x,y,z coordinates and frame_id and time
+        :param message: The ROS message to be converted.
+        :type message: geometry_msgs.msg.Vector3Stamped
+        :return: The frame_id and the vector.
+        :rtype: tuple
+        """
+
+        x = message.vector.x
+        y = message.vector.y
+        z = message.vector.z
+
+        frame_id = message.header.frame_id
+
+        return frame_id, [x, y, z]
+
+    def to_ros_string(self, data: str) -> String:
+        """
+        Creates a String message given data.
+        :param data: The data to be converted.
+        :type data: str
+        :return: ROS message with the data.
+        :rtype: std_msgs.msg.String
+        """
+
+        message = String()
+        message.data = data
+
+        return message
+
+    def from_ros_string(self, message: String):
+        """
+        Creates a String message given data.
+        :param message: The ROS message to be converted.
+        :type message: std_msgs.msg.String
+        :return: The data.
+        :rtype: str
+        """
+
+        return message.data
+
+    def to_ros_transformstamped(self, stamp, frame_id, child_frame_id, odometry):
+        """
+        Creates a TransformStamped message given frame_id, child_frame_id and odometry.
+        """
+        t = TransformStampedMsg()
+        t.header.stamp = stamp
+        t.header.frame_id = frame_id
+        t.child_frame_id = child_frame_id
+        t.transform.translation.x = -odometry[0, 3]
+        t.transform.translation.y = 0.0
+        t.transform.translation.z = -odometry[2, 3]
+        q = tr.quaternion_from_matrix(odometry)
+        t.transform.rotation.x = q[0]
+        t.transform.rotation.y = q[1]
+        t.transform.rotation.z = q[2]
+        t.transform.rotation.w = q[3]
+        return t
+
     def from_ros_transcription(self, ros_transcripton: LiveSpeech) -> VoskTranscription:
         """
         Converts an LiveSpeech object to a VoskTranscription object.

diff --git a/projects/opendr_ws/src/opendr_perception/CMakeLists.txt b/projects/opendr_ws/src/opendr_perception/CMakeLists.txt
@@ -39,7 +39,9 @@ catkin_install_python(PROGRAMS
    scripts/object_detection_2d_gem_node.py
    scripts/semantic_segmentation_bisenet_node.py
    scripts/binary_high_resolution_node.py
-   scripts/object_tracking_2d_siamrpn_node.py
+   scripts/continual_slam_dataset_node.py
+   scripts/continual_slam_predictor_node.py
+   scripts/continual_slam_learner_node.py
    scripts/facial_emotion_estimation_node.py
    scripts/continual_skeleton_based_action_recognition_node.py
    scripts/point_cloud_2_publisher_node.py

diff --git a/projects/opendr_ws/src/opendr_perception/README.md b/projects/opendr_ws/src/opendr_perception/README.md
@@ -879,6 +879,61 @@ whose documentation can be found [here](../../../../docs/reference/audiovisual-e
    For viewing the output, refer to the [notes above.](#notes)
 
 ----
+## RGB + IMU input
+
+### Continual SLAM ROS Nodes
+A ROS node for performing depth+position output mapping based on visual + imu input. Continual SLAM involves the use of two distinct ROS nodes, one dedicated to performing inference and the other exclusively focused on training. Both of the nodes are based on the learner class defined in [ContinualSLAMLearner](../../../../src/opendr/perception/continual_slam/continual_slam_learner.py).
+
+You can find the continual slam ROS node python scripts here [learner](./scripts/continual_slam_learner_node.py), [predictor](./scripts/continual_slam_predictor_node.py). You can further also find the RGB image + IMU publisher node [here](./scripts/continual_slam_dataset_node.py). 
+
+#### Instructions for basic usage:
+
+1. Download the KITTI Visual Odometry datased as it is described [here](../../../../src/opendr/perception/continual_slam/datasets/README.md). 
+
+2. Decide on the frame rate FPS, then one can start the dataset publisher node using the following line:
+   ```shell
+   rosrun opendr_perception continual_slam_dataset_node.py
+   ```
+
+   The following optional arguments are available:
+   - `-h or --help`: show a help message and exit
+   - `--dataset_path`: path to the dataset (default=`./kitti`)
+   - `--config_file_path`: path to the config file for learner class (default=`src/opendr/perception/continual_slam/configs/singlegpu_kitti.yaml`)
+   - `--output_image_topic OUTPUT_IMAGE_TOPIC`: topic to which we are publishing the RGB image (default=`/cl_slam/image`)
+   - `--output_distance_topic OUTPUT_DISTANCE_TOPIC`: topic to publish distances (default=`/cl_slam/distance`)
+   - `--dataset_fps FPS`: frame rate which the dataset will be published, (default=`3`)
+
+3. Start the Predictor Node
+    ```shell
+    rosrun opendr_perception continual_slam_predictor_node.py
+    ```
+    The following optional arguments are available:
+   - `-h or --help`: show a help message and exit
+   - `-c or --config_path`: path to the config file for the learner class (default=`src/opendr/perception/continual_slam/configs/singlegpu_kitti.yaml`)
+   - `-it or --input_image_topic`: input image topic, listened from Continual SLAM Dataset Node (default=`/cl_slam/image`)
+   - `-dt or --input_distance_topic`: input distance topic, listened from Continual SLAM Dataset Node (default=`/cl_slam/distance`)
+   - `-odt or --output_depth_topic`: output depth topic, published to visual output tools (default=`/opendr/predicted/image`)
+   - `-opt or --output_pose_topic`: output pose topic, published to visual output tools (default=`/opendr/predicted/pose`)
+   - `-ppcl or --publish_pointcloud`: boolean to decide whether pointcloud output is asked or not (default=`false`)
+   - `-opct or --output_pointcloud_topic`: output pointcloud topic, depending on `--publish_pointcloud`, published to visual output tools (default=`/opendr/predicted/pointcloud`)
+   - `-ut or --update_topic`: update topic, listened from Continual SLAM Dataset Node (default=`/cl_slam/update`)
+
+4. Start the Learner Node (Optional)
+    ```shell
+    rosrun opendr_perception continual_slam_learner_node.py
+    ```
+    The following optional arguments are available:
+   - `-h or --help`: show a help message and exit
+   - `-c or --config_path`: path to the config file for the learner class (default=`src/opendr/perception/continual_slam/configs/singlegpu_kitti.yaml`)
+   - `-it or --input_image_topic`: input image topic, listened from Continual SLAM Dataset Node (default=`/cl_slam/image`)
+   - `-dt or --input_distance_topic`: input distance topic, listened from Continual SLAM Dataset Node (default=`/cl_slam/distance`)
+   - `-ot or --output_weights_topic`: output weights topic to be published to Continual SLAM Predictor Node (default=`/cl_slam/update`)
+   - `-pr or --publish_rate`: publish rate of the weights (default=`20`)
+   - `-bs or --buffer_size`: size of the replay buffer (default=`10`)
+   - `-ss or --sample_size`: sample size of the replay buffer. If 0 is given, only online data is used (default=`3`)
+   - `-sm or --save_memory`: whether to save memory or not. Add it to the command if you want to write to disk (default=`True`)
+----
+
 ## Audio input
 
 ### Speech Command Recognition ROS Node