negarhdr
diff --git a/‎.gitmodules
+3 b/‎.gitmodules
+3
diff --git a/‎projects/opendr_ws/README.md
+3 b/‎projects/opendr_ws/README.md
+3
diff --git a/‎projects/opendr_ws/src/opendr_bridge/src/opendr_bridge/bridge.py
+186-6 b/‎projects/opendr_ws/src/opendr_bridge/src/opendr_bridge/bridge.py
+186-6
diff --git a/‎projects/opendr_ws/src/opendr_perception/CMakeLists.txt
+3-1 b/‎projects/opendr_ws/src/opendr_perception/CMakeLists.txt
+3-1
diff --git a/‎projects/opendr_ws/src/opendr_perception/README.md
+55 b/‎projects/opendr_ws/src/opendr_perception/README.md
+55
@@ -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
@@ -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)
 
@@ -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.
 
@@ -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
 
@@ -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