Merge remote-tracking branch 'origin/ros' into jetson

2025-01-20 12:44:38 +01:00
parent 23f8df32d5 86d8ccf797
commit ffd182bc07
38 changed files with 32308 additions and 4643 deletions
--- a/extras/mmdeploy/README.md
+++ b/extras/mmdeploy/README.md
@ -70,7 +70,8 @@ mv /mmdeploy/work_dir/end2end.onnx /RapidPoseTriangulation/extras/mmdeploy/expor
 ```

 ```bash
-python3 /RapidPoseTriangulation/extras/mmdeploy/make_extra_graphs.py
+python3 /RapidPoseTriangulation/extras/mmdeploy/make_extra_graphs_pt.py
+python3 /RapidPoseTriangulation/extras/mmdeploy/make_extra_graphs_tf.py
 ```

 ```bash
--- a/extras/mmdeploy/add_extra_steps.py
+++ b/extras/mmdeploy/add_extra_steps.py
@ -54,6 +54,7 @@ def add_steps_to_onnx(model_path):
        inputs=[input_name],
        outputs=[casted_output],
        to=cast_type,
+        name="Cast_Input",
    )

    # Node to transpose
@ -118,6 +119,90 @@ def add_steps_to_onnx(model_path):
    # Set input image type to int8
    model.graph.input[0].type.tensor_type.elem_type = TensorProto.UINT8

+    # Cast all outputs to fp32 to avoid half precision issues in cpp code
+    for output in graph.output:
+        orig_output_name = output.name
+        internal_output_name = orig_output_name + "_internal"
+
+        # Rename the output tensor
+        for node in model.graph.node:
+            for idx, name in enumerate(node.output):
+                if name == orig_output_name:
+                    node.output[idx] = internal_output_name
+
+        # Insert a Cast node that casts the internal output to fp32
+        cast_fp32_name = orig_output_name
+        cast_node_output = helper.make_node(
+            "Cast",
+            inputs=[internal_output_name],
+            outputs=[cast_fp32_name],
+            to=1,
+            name="Cast_Output_" + orig_output_name,
+        )
+        # Append the cast node to the graph
+        graph.node.append(cast_node_output)
+
+        # Update the output's data type info
+        output.type.tensor_type.elem_type = TensorProto.FLOAT
+
+    # Merge the two outputs
+    if "det" in model_path:
+        r1_output = "dets"
+        r2_output = "labels"
+        out_name = "bboxes"
+        out_dim = 6
+    if "pose" in model_path:
+        r1_output = "kpts"
+        r2_output = "scores"
+        out_name = "keypoints"
+        out_dim = 3
+    if "det" in model_path or "pose" in model_path:
+        # Node to expand
+        r2_expanded = r2_output + "_expanded"
+        unsqueeze_node = helper.make_node(
+            "Unsqueeze",
+            inputs=[r2_output],
+            outputs=[r2_expanded],
+            axes=[2],
+            name="Unsqueeze",
+        )
+
+        # Node to concatenate
+        r12_merged = out_name
+        concat_node = helper.make_node(
+            "Concat",
+            inputs=[r1_output, r2_expanded],
+            outputs=[r12_merged],
+            axis=2,
+            name="Merged",
+        )
+
+        # Define the new concatenated output
+        merged_output = helper.make_tensor_value_info(
+            r12_merged,
+            TensorProto.FLOAT,
+            [
+                (
+                    graph.input[0].type.tensor_type.shape.dim[0].dim_value
+                    if graph.input[0].type.tensor_type.shape.dim[0].dim_value > 0
+                    else None
+                ),
+                (
+                    graph.output[0].type.tensor_type.shape.dim[1].dim_value
+                    if graph.output[0].type.tensor_type.shape.dim[1].dim_value > 0
+                    else None
+                ),
+                out_dim,
+            ],
+        )
+
+        # Update the graph
+        graph.node.append(unsqueeze_node)
+        graph.node.append(concat_node)
+        graph.output.pop()
+        graph.output.pop()
+        graph.output.append(merged_output)
+
    path = re.sub(r"(x)(\d+)x(\d+)x(\d+)", r"\1\3x\4x\2", model_path)
    path = path.replace(".onnx", "_extra-steps.onnx")
    onnx.save(model, path)
--- a/extras/mmdeploy/dockerfile
+++ b/extras/mmdeploy/dockerfile
@ -34,5 +34,8 @@ RUN pip3 install --upgrade --no-cache-dir onnxconverter_common
 # Fix an error when profiling
 RUN pip3 install --upgrade --no-cache-dir "onnxruntime-gpu<1.17"

+RUN pip3 install --upgrade --no-cache-dir tensorflow
+RUN pip3 install --upgrade --no-cache-dir tf2onnx
+
 WORKDIR /mmdeploy/
 CMD ["/bin/bash"]
--- a/extras/mmdeploy/make_extra_graphs_pt.py
+++ b/extras/mmdeploy/make_extra_graphs_pt.py
--- a/extras/mmdeploy/make_extra_graphs_tf.py
+++ b/extras/mmdeploy/make_extra_graphs_tf.py
@ -0,0 +1,276 @@
+import cv2
+
+import numpy as np
+import tensorflow as tf
+import tf2onnx
+
+# ==================================================================================================
+
+base_path = "/RapidPoseTriangulation/extras/mmdeploy/exports/"
+det_target_size = (320, 320)
+
+# ==================================================================================================
+
+
+class BayerToRGB(tf.keras.layers.Layer):
+    """Convert Bayer image to RGB
+    See: https://stanford.edu/class/ee367/reading/Demosaicing_ICASSP04.pdf
+    See: https://github.com/cheind/pytorch-debayer/blob/master/debayer/modules.py#L231
+    """
+
+    def __init__(self):
+        super().__init__()
+        self.layout = "RGGB"
+        self.max_val = 255.0
+
+        self.kernels = tf.constant(
+            np.array(
+                [
+                    # G at R/B locations
+                    [
+                        [0, 0, -1, 0, 0],
+                        [0, 0, 2, 0, 0],
+                        [-1, 2, 4, 2, -1],
+                        [0, 0, 2, 0, 0],
+                        [0, 0, -1, 0, 0],
+                    ],
+                    # R/B at G in R/B rows and B/R columns
+                    [
+                        [0, 0, 0.5, 0, 0],
+                        [0, -1, 0, -1, 0],
+                        [-1, 4, 5, 4, -1],
+                        [0, -1, 0, -1, 0],
+                        [0, 0, 0.5, 0, 0],
+                    ],
+                    # R/B at G in B/R rows and R/B columns
+                    [
+                        [0, 0, 0.5, 0, 0],
+                        [0, -1, 4, -1, 0],
+                        [-1, 0, 5, 0, -1],
+                        [0, -1, 4, -1, 0],
+                        [0, 0, 0.5, 0, 0],
+                    ],
+                    # R/B at B/R in B/R rows and B/R columns
+                    [
+                        [0, 0, -1.5, 0, 0],
+                        [0, 2, 0, 2, 0],
+                        [-1.5, 0, 6, 0, -1.5],
+                        [0, 2, 0, 2, 0],
+                        [0, 0, -1.5, 0, 0],
+                    ],
+                ],
+                dtype=np.float32,
+            )
+            .reshape(1, 4, 5, 5)
+            .transpose(2, 3, 0, 1)
+            / 8.0
+        )
+        self.index = tf.constant(
+            np.array(
+                # Describes the kernel indices that calculate the corresponding RGB values for
+                # the 2x2 layout (RGGB)  sub-structure
+                [
+                    # Destination R
+                    [
+                        [4, 1],  # identity, R at G in R row  B column
+                        [2, 3],  # R at G in B row R column, R at B in B row R column
+                    ],
+                    # Destination G
+                    [
+                        [0, 4],
+                        [4, 0],
+                    ],
+                    # Destination B
+                    [
+                        [3, 2],
+                        [1, 4],
+                    ],
+                ]
+            ).reshape(1, 3, 2, 2)
+        )
+
+    def call(self, img):
+        H, W = tf.shape(img)[1], tf.shape(img)[2]
+
+        # Pad the image
+        tpad = img[:, 0:2, :, :]
+        bpad = img[:, H - 2 : H, :, :]
+        ipad = tf.concat([tpad, img, bpad], axis=1)
+        lpad = ipad[:, :, 0:2, :]
+        rpad = ipad[:, :, W - 2 : W, :]
+        ipad = tf.concat([lpad, ipad, rpad], axis=2)
+
+        # Convolve with kernels
+        planes = tf.nn.conv2d(ipad, self.kernels, strides=[1, 1, 1, 1], padding="VALID")
+
+        # Concatenate identity kernel
+        planes = tf.concat([planes, img], axis=-1)
+
+        # Gather values
+        index_repeated = tf.tile(self.index, multiples=[1, 1, H // 2, W // 2])
+        index_repeated = tf.transpose(index_repeated, perm=[0, 2, 3, 1])
+        row_indices, col_indices = tf.meshgrid(tf.range(H), tf.range(W), indexing="ij")
+        index_tensor = tf.stack([row_indices, col_indices], axis=-1)
+        index_tensor = tf.expand_dims(index_tensor, axis=0)
+        index_tensor = tf.expand_dims(index_tensor, axis=-2)
+        index_tensor = tf.repeat(index_tensor, repeats=3, axis=-2)
+        index_repeated = tf.expand_dims(index_repeated, axis=-1)
+        indices = tf.concat([tf.cast(index_tensor, tf.int64), index_repeated], axis=-1)
+        rgb = tf.gather_nd(planes, indices, batch_dims=1)
+
+        if self.max_val == 255.0:
+            # Make value range valid again
+            rgb = tf.round(rgb)
+
+        return rgb
+
+
+# ==================================================================================================
+
+
+def bayer_resize(img, size):
+    """Resize a Bayer image by splitting color channels"""
+
+    # Split the image into 4 channels
+    r = img[:, 0::2, 0::2, 0]
+    g1 = img[:, 0::2, 1::2, 0]
+    g2 = img[:, 1::2, 0::2, 0]
+    b = img[:, 1::2, 1::2, 0]
+    bsplit = tf.stack([r, g1, g2, b], axis=-1)
+
+    # Resize the image
+    # Make sure the target size is divisible by 2
+    size = (size[0] // 2, size[1] // 2)
+    bsized = tf.image.resize(bsplit, size=size, method="bilinear")
+
+    # Create a bayer image again
+    img = tf.nn.depth_to_space(bsized, block_size=2)
+
+    return img
+
+
+# ==================================================================================================
+
+
+class Letterbox(tf.keras.layers.Layer):
+    def __init__(self, target_size, fill_value=128):
+        """Resize and pad image while keeping aspect ratio"""
+        super(Letterbox, self).__init__()
+
+        self.b2rgb = BayerToRGB()
+        self.target_size = target_size
+        self.fill_value = fill_value
+
+    def calc_params(self, ishape):
+        img_h, img_w = ishape[1], ishape[2]
+        target_h, target_w = self.target_size
+
+        scale = tf.minimum(target_w / img_w, target_h / img_h)
+        new_w = tf.round(tf.cast(img_w, scale.dtype) * scale)
+        new_h = tf.round(tf.cast(img_h, scale.dtype) * scale)
+        new_w = tf.cast(new_w, tf.int32)
+        new_h = tf.cast(new_h, tf.int32)
+        new_w = new_w - (new_w % 2)
+        new_h = new_h - (new_h % 2)
+
+        pad_w = target_w - new_w
+        pad_h = target_h - new_h
+        pad_left = tf.cast(tf.floor(tf.cast(pad_w, tf.float32) / 2.0), tf.int32)
+        pad_top = tf.cast(tf.floor(tf.cast(pad_h, tf.float32) / 2.0), tf.int32)
+        pad_right = pad_w - pad_left
+        pad_bottom = pad_h - pad_top
+        paddings = [pad_top, pad_bottom, pad_left, pad_right]
+
+        return paddings, scale, (new_w, new_h)
+
+    def call(self, img):
+        paddings, _, (nw, nh) = self.calc_params(tf.shape(img))
+
+        # Resize the image and convert to RGB
+        img = bayer_resize(img, (nh, nw))
+        img = self.b2rgb(img)
+
+        # Pad the image
+        pad_top, pad_bottom, pad_left, pad_right = paddings
+        img = tf.pad(
+            img,
+            paddings=[[0, 0], [pad_top, pad_bottom], [pad_left, pad_right], [0, 0]],
+            mode="CONSTANT",
+            constant_values=self.fill_value,
+        )
+
+        return img
+
+
+# ==================================================================================================
+
+
+class DetPreprocess(tf.keras.layers.Layer):
+    def __init__(self, target_size, fill_value=114):
+        super(DetPreprocess, self).__init__()
+        self.letterbox = Letterbox(target_size, fill_value)
+
+    def call(self, img):
+        """img: tf.Tensor of shape [batch, H, W, C], dtype=tf.uint8"""
+
+        # Cast to float32 since TensorRT does not support uint8 layers
+        img = tf.cast(img, tf.float32)
+
+        img = self.letterbox(img)
+        return img
+
+
+# ==================================================================================================
+
+
+def rgb2bayer(img):
+    bayer = np.zeros((img.shape[0], img.shape[1]), dtype=img.dtype)
+    bayer[0::2, 0::2] = img[0::2, 0::2, 0]
+    bayer[0::2, 1::2] = img[0::2, 1::2, 1]
+    bayer[1::2, 0::2] = img[1::2, 0::2, 1]
+    bayer[1::2, 1::2] = img[1::2, 1::2, 2]
+    return bayer
+
+
+# ==================================================================================================
+
+
+def main():
+
+    img_path = "/RapidPoseTriangulation/scripts/../data/h1/54138969-img_003201.jpg"
+    image = cv2.imread(img_path, 3)
+    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+    image = rgb2bayer(image)
+    image = np.expand_dims(image, axis=-1)
+    image = np.asarray(image, dtype=np.uint8)
+
+    # Initialize the DetPreprocess module
+    preprocess_model = tf.keras.Sequential()
+    preprocess_model.add(DetPreprocess(target_size=det_target_size))
+    det_dummy_input_a0 = tf.convert_to_tensor(
+        np.expand_dims(image, axis=0), dtype=tf.uint8
+    )
+    det_dummy_output_a0 = preprocess_model(det_dummy_input_a0)
+    print("\n", det_dummy_output_a0.shape, "\n")
+
+    output_a0 = det_dummy_output_a0.numpy()
+    output_a0 = np.squeeze(output_a0, axis=0)
+    output_a0 = np.asarray(output_a0, dtype=np.uint8)
+    output_a0 = cv2.cvtColor(output_a0, cv2.COLOR_RGB2BGR)
+    cv2.imwrite(base_path + "det_preprocess.jpg", output_a0)
+
+    # Export to ONNX
+    input_signature = [tf.TensorSpec([None, None, None, 1], tf.uint8, name="x")]
+    _, _ = tf2onnx.convert.from_keras(
+        preprocess_model,
+        input_signature,
+        opset=11,
+        output_path=base_path + "det_preprocess.onnx",
+        target=["tensorrt"],
+    )
+
+
+# ==================================================================================================
+
+if __name__ == "__main__":
+    main()
--- a/media/RESULTS.md
+++ b/media/RESULTS.md
--- a/ros/README.md
+++ b/ros/README.md
@ -0,0 +1,45 @@
+# ROS-Wrapper
+
+Run pose estimator with ros topics as inputs and publish detected poses.
+
+<br>
+
+- Build container:
+  ```bash
+  docker build --progress=plain -t rapidposetriangulation_ros -f ros/dockerfile .
+  ```
+
+- Update or remove the `ROS_DOMAIN_ID` in `docker-compose.yml` to fit your environment
+
+- Run and test:
+  ```bash
+  xhost +; docker compose -f ros/docker-compose.yml up
+
+  docker exec -it ros-test_node-1 bash
+  export ROS_DOMAIN_ID=18
+  ```
+
+### Debugging
+
+```bash
+cd /RapidPoseTriangulation/ros/rpt2D_wrapper_cpp/tests/
+
+g++ -std=c++17 -O3 -march=native -Wall -DCOMPILE_EXAMPLE_MAIN \
+    $(pkg-config --cflags opencv4) \
+    -I /onnxruntime/include \
+    -I /onnxruntime/include/onnxruntime/core/session \
+    -I /onnxruntime/include/onnxruntime/core/providers/tensorrt \
+    -L /onnxruntime/build/Linux/Release \
+    my_app.cpp \
+    -o my_app \
+    -Wl,--start-group \
+      -lonnxruntime_providers_tensorrt \
+      -lonnxruntime_providers_shared \
+      -lonnxruntime_providers_cuda \
+      -lonnxruntime \
+    -Wl,--end-group \
+    $(pkg-config --libs opencv4) \
+    -Wl,-rpath,/onnxruntime/build/Linux/Release
+
+./my_app
+```
--- a/ros/docker-compose.yml
+++ b/ros/docker-compose.yml
@ -0,0 +1,71 @@
+version: "2.3"
+# runtime: nvidia needs version 2 else change standard runtime at host pc
+
+services:
+
+    test_node:
+        image: rapidposetriangulation_ros
+        network_mode: "host"
+        ipc: "host"
+        runtime: nvidia
+        privileged: true
+        volumes:
+            - ../:/RapidPoseTriangulation/
+            - ../skelda/:/skelda/
+            - /tmp/.X11-unix:/tmp/.X11-unix
+            - /dev/shm:/dev/shm
+        environment:
+            - DISPLAY
+            - QT_X11_NO_MITSHM=1
+            - "PYTHONUNBUFFERED=1"
+        command: /bin/bash -i -c 'sleep infinity'
+
+    estimator:
+        image: rapidposetriangulation_ros
+        network_mode: "host"
+        ipc: "host"
+        runtime: nvidia
+        privileged: true
+        volumes:
+            - ../:/RapidPoseTriangulation/
+            - ../skelda/:/skelda/
+            - /tmp/.X11-unix:/tmp/.X11-unix
+            - /dev/shm:/dev/shm
+        environment:
+            - DISPLAY
+            - QT_X11_NO_MITSHM=1
+            - "PYTHONUNBUFFERED=1"
+        # command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt2D_wrapper_py rpt2D_wrapper'
+        command: /bin/bash -i -c 'export ROS_DOMAIN_ID=18 && ros2 run rpt2D_wrapper_cpp rpt2D_wrapper'
+
+    pose_visualizer:
+        image: rapidposetriangulation_ros
+        network_mode: "host"
+        ipc: "host"
+        runtime: nvidia
+        privileged: true
+        volumes:
+            - ../:/RapidPoseTriangulation/
+            - ../skelda/:/skelda/
+            - /tmp/.X11-unix:/tmp/.X11-unix
+            - /dev/shm:/dev/shm
+        environment:
+            - DISPLAY
+            - QT_X11_NO_MITSHM=1
+            - "PYTHONUNBUFFERED=1"
+        command: /bin/bash -i -c 'sleep 2 && export ROS_DOMAIN_ID=18 && ros2 run pose2D_visualizer pose2D_visualizer'
+
+    pose_viewer:
+        image: rapidposetriangulation_ros
+        network_mode: "host"
+        ipc: "host"
+        runtime: nvidia
+        privileged: true
+        volumes:
+            - /tmp/.X11-unix:/tmp/.X11-unix
+            - /dev/shm:/dev/shm
+        environment:
+            - DISPLAY
+            - QT_X11_NO_MITSHM=1
+            - "PYTHONUNBUFFERED=1"
+        command: /bin/bash -i -c 'sleep 2 && export ROS_DOMAIN_ID=18 && ros2 run image_view image_view --ros-args --remap image:=/camera01/img_with_pose -p autosize:=True -p window_name:=MyImage'
--- a/ros/dockerfile
+++ b/ros/dockerfile
@ -0,0 +1,82 @@
+FROM rapidposetriangulation
+WORKDIR /
+
+# Install ONNX runtime
+# See: https://github.com/microsoft/onnxruntime/blob/main/dockerfiles/Dockerfile.tensorrt
+RUN pip3 uninstall -y onnxruntime-gpu
+RUN git clone --recursive --depth=1 --branch=v1.20.1 https://github.com/Microsoft/onnxruntime.git
+ENV PATH=/usr/local/nvidia/bin:/usr/local/cuda/bin:/cmake-3.30.1-linux-x86_64/bin:${PATH}
+ARG CMAKE_CUDA_ARCHITECTURES=75;80;90
+ENV TRT_VERSION=10.5.0.18
+RUN /bin/sh onnxruntime/dockerfiles/scripts/install_common_deps.sh
+RUN /bin/sh onnxruntime/dockerfiles/scripts/checkout_submodules.sh ${trt_version}
+RUN ls
+RUN cd onnxruntime && \
+    /bin/sh build.sh --allow_running_as_root --parallel --build_shared_lib \
+      --cuda_home /usr/local/cuda --cudnn_home /usr/lib/x86_64-linux-gnu/ --use_tensorrt \
+      --tensorrt_home /usr/lib/x86_64-linux-gnu/ --config Release --build_wheel --skip_tests \
+      --skip_submodule_sync --cmake_extra_defines '"CMAKE_CUDA_ARCHITECTURES='${CMAKE_CUDA_ARCHITECTURES}'"'
+RUN cd onnxruntime && pip install build/Linux/Release/dist/*.whl
+
+# Install ROS2
+# https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html
+RUN apt-get update && apt-get install -y --no-install-recommends locales
+RUN locale-gen en_US en_US.UTF-8 && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
+RUN apt-get update && apt-get install -y --no-install-recommends software-properties-common
+RUN add-apt-repository universe
+RUN apt-get update && apt-get install -y --no-install-recommends curl
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" > /etc/apt/sources.list.d/ros2.list
+RUN apt-get update && apt-get install -y --no-install-recommends ros-humble-ros-base python3-argcomplete
+RUN apt-get update && apt-get install -y --no-install-recommends ros-dev-tools
+RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
+
+# Create ROS2 workspace for basic packages
+RUN mkdir -p /project/base/src/
+RUN cd /project/base/; colcon build
+RUN echo "source /project/base/install/setup.bash" >> ~/.bashrc
+
+# Install opencv and cv_bridge
+RUN apt-get update && apt-get install -y --no-install-recommends libboost-dev
+RUN apt-get update && apt-get install -y --no-install-recommends libboost-python-dev
+RUN apt-get update && apt-get install -y --no-install-recommends libopencv-dev
+RUN cd /project/base/src/; git clone --branch humble --depth=1 https://github.com/ros-perception/vision_opencv.git
+RUN /bin/bash -i -c 'cd /project/base/; colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release'
+
+# Install ROS2 image viewer
+RUN cd /project/base/src/; git clone --branch=humble --depth=1 https://github.com/ros-perception/image_pipeline.git
+RUN cd /project/base/src/; git clone --branch=humble --depth=1 https://github.com/ros-perception/image_common.git
+RUN /bin/bash -i -c 'cd /project/base/; colcon build --symlink-install --packages-select camera_calibration_parsers image_transport image_view --cmake-args -DCMAKE_BUILD_TYPE=Release'
+
+# Fix module not found error when displaying images
+RUN apt-get update && apt-get install -y --no-install-recommends libcanberra-gtk-module libcanberra-gtk3-module
+
+# Create ROS2 workspace for project packages
+RUN mkdir -p /project/dev_ws/src/
+RUN cd /project/dev_ws/; colcon build
+RUN echo "source /project/dev_ws/install/setup.bash" >> ~/.bashrc
+
+# Fix ros package building error
+RUN pip3 install --no-cache-dir "setuptools<=58.2.0"
+
+# Copy modules
+COPY ./ros/pose2D_visualizer /RapidPoseTriangulation/ros/pose2D_visualizer/
+COPY ./ros/rpt2D_wrapper_py /RapidPoseTriangulation/ros/rpt2D_wrapper_py/
+COPY ./ros/rpt2D_wrapper_cpp /RapidPoseTriangulation/ros/rpt2D_wrapper_cpp/
+
+# Link and build as ros package
+RUN ln -s  /RapidPoseTriangulation/ros/pose2D_visualizer/ /project/dev_ws/src/pose2D_visualizer
+RUN ln -s  /RapidPoseTriangulation/ros/rpt2D_wrapper_py/ /project/dev_ws/src/rpt2D_wrapper_py
+RUN ln -s  /RapidPoseTriangulation/ros/rpt2D_wrapper_cpp/ /project/dev_ws/src/rpt2D_wrapper_cpp
+RUN /bin/bash -i -c 'cd /project/dev_ws/; colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release'
+
+# Update ros packages -> autocompletion and check
+RUN /bin/bash -i -c 'ros2 pkg list'
+
+# Clear cache to save space, only has an effect if image is squashed
+RUN apt-get autoremove -y \
+  && apt-get clean \
+  && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /RapidPoseTriangulation/
+CMD ["/bin/bash"]
--- a/ros/pose2D_visualizer/package.xml
+++ b/ros/pose2D_visualizer/package.xml
@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pose2D_visualizer</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
--- a/ros/pose2D_visualizer/pose2D_visualizer/init.py
+++ b/ros/pose2D_visualizer/pose2D_visualizer/init.py
--- a/ros/pose2D_visualizer/pose2D_visualizer/pose2D_visualizer.py
+++ b/ros/pose2D_visualizer/pose2D_visualizer/pose2D_visualizer.py
@ -0,0 +1,152 @@
+import json
+import os
+import sys
+import threading
+import time
+
+import cv2
+from matplotlib import pyplot as plt
+import numpy as np
+import rclpy
+from cv_bridge import CvBridge
+from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+filepath = os.path.dirname(os.path.realpath(__file__)) + "/"
+sys.path.append(filepath + "../../../scripts/")
+import test_triangulate
+
+from skelda import utils_view
+
+# ==================================================================================================
+
+bridge = CvBridge()
+node = None
+publisher_img = None
+
+cam_id = "camera01"
+img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw"
+pose_input_topic = "/poses/" + cam_id
+img_output_topic = "/" + cam_id + "/img_with_pose"
+
+last_input_image = None
+lock = threading.Lock()
+
+# ==================================================================================================
+
+
+def callback_images(image_data):
+    global last_input_image, lock
+
+    # Convert into cv images from image string
+    if image_data.encoding == "bayer_rggb8":
+        bayer_image = bridge.imgmsg_to_cv2(image_data, "bayer_rggb8")
+        color_image = test_triangulate.bayer2rgb(bayer_image)
+    elif image_data.encoding == "mono8":
+        gray_image = bridge.imgmsg_to_cv2(image_data, "mono8")
+        color_image = cv2.cvtColor(gray_image, cv2.COLOR_GRAY2RGB)
+    elif image_data.encoding == "rgb8":
+        color_image = bridge.imgmsg_to_cv2(image_data, "rgb8")
+    else:
+        raise ValueError("Unknown image encoding:", image_data.encoding)
+
+    time_stamp = image_data.header.stamp.sec + image_data.header.stamp.nanosec / 1.0e9
+
+    with lock:
+        last_input_image = (color_image, time_stamp)
+
+
+# ==================================================================================================
+
+
+def callback_poses(pose_data):
+    global last_input_image, lock
+
+    ptime = time.time()
+    if last_input_image is None:
+        return
+
+    # Convert pose data from json string
+    poses = json.loads(pose_data.data)
+
+    # Collect inputs
+    images_2d = []
+    timestamps = []
+    with lock:
+        img = np.copy(last_input_image[0])
+        ts = last_input_image[1]
+        images_2d.append(img)
+        timestamps.append(ts)
+
+    # Visualize 2D poses
+    bodies2D = poses["bodies2D"]
+    colors = plt.cm.hsv(np.linspace(0, 1, len(bodies2D), endpoint=False)).tolist()
+    colors = [[int(c[0] * 255), int(c[1] * 255), int(c[2] * 255)] for c in colors]
+    for i, body in enumerate(bodies2D):
+        color = list(reversed(colors[i]))
+        img = utils_view.draw_body_in_image(img, body, poses["joints"], color)
+
+    # Publish image with poses
+    publish(img)
+
+    msg = "Visualization time: {:.3f}s"
+    print(msg.format(time.time() - ptime))
+
+
+# ==================================================================================================
+
+
+def publish(img):
+    # Publish image data
+    msg = bridge.cv2_to_imgmsg(img, "rgb8")
+    publisher_img.publish(msg)
+
+
+# ==================================================================================================
+
+
+def main():
+    global node, publisher_img
+
+    # Start node
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("pose2D_visualizer")
+
+    # Quality of service settings
+    qos_profile = QoSProfile(
+        reliability=QoSReliabilityPolicy.RELIABLE,
+        history=QoSHistoryPolicy.KEEP_LAST,
+        depth=1,
+    )
+
+    # Create subscribers
+    _ = node.create_subscription(
+        Image,
+        img_input_topic,
+        callback_images,
+        qos_profile,
+    )
+    _ = node.create_subscription(
+        String,
+        pose_input_topic,
+        callback_poses,
+        qos_profile,
+    )
+
+    # Create publishers
+    publisher_img = node.create_publisher(Image, img_output_topic, qos_profile)
+
+    node.get_logger().info("Finished initialization of pose visualizer")
+
+    # Run ros update thread
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+# ==================================================================================================
+
+if __name__ == "__main__":
+    main()
--- a/ros/pose2D_visualizer/resource/pose2D_visualizer
+++ b/ros/pose2D_visualizer/resource/pose2D_visualizer
--- a/ros/pose2D_visualizer/setup.cfg
+++ b/ros/pose2D_visualizer/setup.cfg
@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pose2D_visualizer
+[install]
+install_scripts=$base/lib/pose2D_visualizer
--- a/ros/pose2D_visualizer/setup.py
+++ b/ros/pose2D_visualizer/setup.py
@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = "pose2D_visualizer"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="root",
+    maintainer_email="root@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": ["pose2D_visualizer = pose2D_visualizer.pose2D_visualizer:main"],
+    },
+)
--- a/ros/pose2D_visualizer/test/test_copyright.py
+++ b/ros/pose2D_visualizer/test/test_copyright.py
@ -0,0 +1,27 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+from ament_copyright.main import main
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
--- a/ros/pose2D_visualizer/test/test_flake8.py
+++ b/ros/pose2D_visualizer/test/test_flake8.py
@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+from ament_flake8.main import main_with_errors
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
--- a/ros/pose2D_visualizer/test/test_pep257.py
+++ b/ros/pose2D_visualizer/test/test_pep257.py
@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+from ament_pep257.main import main
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
--- a/ros/rpt2D_wrapper_cpp/CMakeLists.txt
+++ b/ros/rpt2D_wrapper_cpp/CMakeLists.txt
@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.5)
+project(rpt2D_wrapper_cpp)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+
+### 3) ONNX Runtime 
+include_directories(/onnxruntime/include
+                    /onnxruntime/include/onnxruntime/core/session
+                    /onnxruntime/include/onnxruntime/core/providers/tensorrt)
+link_directories(/onnxruntime/build/Linux/Release)
+
+add_executable(rpt2D_wrapper src/rpt2D_wrapper.cpp)
+ament_target_dependencies(rpt2D_wrapper rclcpp std_msgs sensor_msgs cv_bridge)
+target_include_directories(rpt2D_wrapper PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+target_link_libraries(rpt2D_wrapper
+  ${OpenCV_LIBS}
+  onnxruntime_providers_tensorrt
+  onnxruntime_providers_shared
+  onnxruntime_providers_cuda
+  onnxruntime
+)
+
+set_target_properties(rpt2D_wrapper PROPERTIES
+  BUILD_WITH_INSTALL_RPATH TRUE
+  INSTALL_RPATH "/onnxruntime/build/Linux/Release"
+)
+
+install(TARGETS rpt2D_wrapper
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
--- a/ros/rpt2D_wrapper_cpp/include/nlohmann/json.hpp
+++ b/ros/rpt2D_wrapper_cpp/include/nlohmann/json.hpp
--- a/ros/rpt2D_wrapper_cpp/package.xml
+++ b/ros/rpt2D_wrapper_cpp/package.xml
@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rpt2D_wrapper_cpp</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <depend>cv_bridge</depend>
+  <depend>OpenCV</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/ros/rpt2D_wrapper_cpp/src/rpt2D_wrapper.cpp
+++ b/ros/rpt2D_wrapper_cpp/src/rpt2D_wrapper.cpp
@ -0,0 +1,261 @@
+#include <chrono>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+#include <mutex>
+#include <atomic>
+#include <cmath>
+#include <iostream>
+
+// ROS2
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/string.hpp>
+
+// OpenCV / cv_bridge
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+// JSON library
+#include "nlohmann/json.hpp"
+using json = nlohmann::json;
+
+#include "test_triangulate.hpp"
+#include "utils_2d_pose.hpp"
+
+// =================================================================================================
+
+static const std::string cam_id = "camera01";
+static const std::string img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw";
+static const std::string pose_out_topic = "/poses/" + cam_id;
+
+static const float min_bbox_score = 0.4;
+static const float min_bbox_area = 0.1 * 0.1;
+static const bool batch_poses = true;
+
+// =================================================================================================
+// =================================================================================================
+
+class Rpt2DWrapperNode : public rclcpp::Node
+{
+public:
+    Rpt2DWrapperNode(const std::string &node_name)
+        : Node(node_name)
+    {
+        this->stop_flag = false;
+        this->last_input_image = cv::Mat();
+        this->last_input_time = 0.0;
+
+        // QoS
+        rclcpp::QoS qos_profile(1);
+        qos_profile.reliable();
+        qos_profile.keep_last(1);
+
+        // Setup subscriber
+        image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
+            img_input_topic, qos_profile,
+            std::bind(&Rpt2DWrapperNode::callbackImages, this, std::placeholders::_1));
+
+        // Setup publisher
+        pose_pub_ = this->create_publisher<std_msgs::msg::String>(pose_out_topic, qos_profile);
+
+        // Load model
+        bool whole_body = test_triangulate::use_whole_body();
+        this->kpt_model = std::make_unique<utils_2d_pose::PosePredictor>(
+            whole_body, min_bbox_score, min_bbox_area, batch_poses);
+
+        RCLCPP_INFO(this->get_logger(), "Finished initialization of pose estimator.");
+
+        // Start background prediction thread
+        model_thread = std::thread(&Rpt2DWrapperNode::callbackWrapper, this);
+    }
+
+    ~Rpt2DWrapperNode()
+    {
+        stop_flag = true;
+        if (model_thread.joinable())
+        {
+            model_thread.join();
+        }
+    }
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_pub_;
+
+    // Pose model pointer
+    std::unique_ptr<utils_2d_pose::PosePredictor> kpt_model;
+
+    // Threading
+    std::thread model_thread;
+    std::mutex mutex;
+    std::atomic<bool> stop_flag;
+
+    cv::Mat last_input_image;
+    double last_input_time;
+
+    void callbackImages(const sensor_msgs::msg::Image::SharedPtr msg);
+    void callbackModel();
+
+    void callbackWrapper()
+    {
+        using namespace std::chrono_literals;
+        while (!stop_flag)
+        {
+            callbackModel();
+            std::this_thread::sleep_for(std::chrono::microseconds(100));
+        }
+    }
+
+    void publish(const json &data)
+    {
+        std_msgs::msg::String msg;
+        msg.data = data.dump();
+        pose_pub_->publish(msg);
+    }
+};
+
+// =================================================================================================
+
+void Rpt2DWrapperNode::callbackImages(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+    // Load or convert image to Bayer format
+    cv::Mat bayer_image;
+    try
+    {
+        if (msg->encoding == "mono8")
+        {
+            cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, msg->encoding);
+            bayer_image = cv_ptr->image;
+        }
+        else if (msg->encoding == "bayer_rggb8")
+        {
+            cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, msg->encoding);
+            bayer_image = cv_ptr->image;
+        }
+        else if (msg->encoding == "rgb8")
+        {
+            cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, "rgb8");
+            cv::Mat color_image = cv_ptr->image;
+            bayer_image = test_triangulate::rgb2bayer(color_image);
+        }
+        else
+        {
+            throw std::runtime_error("Unknown image encoding: " + msg->encoding);
+        }
+    }
+    catch (const std::exception &e)
+    {
+        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
+        return;
+    }
+
+    // Get timestamp
+    double time_stamp = msg->header.stamp.sec + msg->header.stamp.nanosec / 1.0e9;
+
+    // Store in member variables with lock
+    {
+        std::lock_guard<std::mutex> lk(mutex);
+        this->last_input_image = std::move(bayer_image);
+        this->last_input_time = time_stamp;
+    }
+}
+
+// =================================================================================================
+
+void Rpt2DWrapperNode::callbackModel()
+{
+    auto ptime = std::chrono::high_resolution_clock::now();
+
+    // Check if we have an image
+    cv::Mat local_image;
+    double local_timestamp = 0.0;
+    {
+        std::lock_guard<std::mutex> lk(mutex);
+        if (last_input_time == 0.0)
+        {
+            return;
+        }
+        local_image = std::move(last_input_image);
+        local_timestamp = last_input_time;
+
+        last_input_image = cv::Mat();
+        last_input_time = 0.0;
+    }
+
+    // Create image vector
+    cv::Mat rgb_image = test_triangulate::bayer2rgb(local_image);
+    std::vector<cv::Mat> images_2d;
+    images_2d.push_back(rgb_image);
+
+    // Predict 2D poses
+    auto poses_2d_all = kpt_model->predict(images_2d);
+    auto poses_2d_upd = test_triangulate::update_keypoints(
+        poses_2d_all, test_triangulate::joint_names_2d);
+    auto &poses_2d = poses_2d_upd[0];
+
+    // Drop persons with no joints
+    std::vector<std::vector<std::array<float, 3>>> valid_poses;
+    for (auto &person : poses_2d)
+    {
+        float sum_conf = 0.0;
+        for (auto &kp : person)
+        {
+            sum_conf += kp[2];
+        }
+        if (sum_conf > 0.0)
+        {
+            valid_poses.push_back(person);
+        }
+    }
+
+    // Round poses to 3 decimal places
+    for (auto &person : valid_poses)
+    {
+        for (auto &kp : person)
+        {
+            kp[0] = std::round(kp[0] * 1000.0) / 1000.0;
+            kp[1] = std::round(kp[1] * 1000.0) / 1000.0;
+            kp[2] = std::round(kp[2] * 1000.0) / 1000.0;
+        }
+    }
+
+    // Build JSON
+    auto ts_pose = std::chrono::high_resolution_clock::now();
+    double ts_pose_sec = std::chrono::duration<double>(ts_pose.time_since_epoch()).count();
+    double z_images_pose = ts_pose_sec - local_timestamp;
+
+    json poses_msg;
+    poses_msg["bodies2D"] = valid_poses; // shape: persons x keypoints x 3
+    poses_msg["joints"] = test_triangulate::joint_names_2d;
+    poses_msg["num_persons"] = valid_poses.size();
+    poses_msg["timestamps"] = {
+        {"image", local_timestamp},
+        {"pose", ts_pose_sec},
+        {"z-images-pose", z_images_pose}};
+
+    // Publish
+    publish(poses_msg);
+
+    // Print info
+    double elapsed_time = std::chrono::duration<double>(
+                              std::chrono::high_resolution_clock::now() - ptime)
+                              .count();
+    std::cout << "Detected persons: " << valid_poses.size()
+              << " - Prediction time: " << elapsed_time << "s" << std::endl;
+}
+
+// =================================================================================================
+// =================================================================================================
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<Rpt2DWrapperNode>("rpt2D_wrapper");
+    rclcpp::spin(node);
+
+    rclcpp::shutdown();
+    return 0;
+}
--- a/ros/rpt2D_wrapper_cpp/src/test_triangulate.hpp
+++ b/ros/rpt2D_wrapper_cpp/src/test_triangulate.hpp
@ -0,0 +1,296 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <opencv2/opencv.hpp>
+
+// =================================================================================================
+
+namespace test_triangulate
+{
+    // If any part shall be disabled, also update the joint names list
+    static const std::map<std::string, bool> whole_body = {
+        {"foots", true},
+        {"face", true},
+        {"hands", true},
+    };
+
+    static const std::vector<std::string> joint_names_2d = {
+        // coco
+        "nose",
+        "eye_left",
+        "eye_right",
+        "ear_left",
+        "ear_right",
+        "shoulder_left",
+        "shoulder_right",
+        "elbow_left",
+        "elbow_right",
+        "wrist_left",
+        "wrist_right",
+        "hip_left",
+        "hip_right",
+        "knee_left",
+        "knee_right",
+        "ankle_left",
+        "ankle_right",
+        // foot
+        "foot_toe_big_left",
+        "foot_toe_small_left",
+        "foot_heel_left",
+        "foot_toe_big_right",
+        "foot_toe_small_right",
+        "foot_heel_right",
+        // face
+        "face_jaw_right_1",
+        "face_jaw_right_2",
+        "face_jaw_right_3",
+        "face_jaw_right_4",
+        "face_jaw_right_5",
+        "face_jaw_right_6",
+        "face_jaw_right_7",
+        "face_jaw_right_8",
+        "face_jaw_middle",
+        "face_jaw_left_1",
+        "face_jaw_left_2",
+        "face_jaw_left_3",
+        "face_jaw_left_4",
+        "face_jaw_left_5",
+        "face_jaw_left_6",
+        "face_jaw_left_7",
+        "face_jaw_left_8",
+        "face_eyebrow_right_1",
+        "face_eyebrow_right_2",
+        "face_eyebrow_right_3",
+        "face_eyebrow_right_4",
+        "face_eyebrow_right_5",
+        "face_eyebrow_left_1",
+        "face_eyebrow_left_2",
+        "face_eyebrow_left_3",
+        "face_eyebrow_left_4",
+        "face_eyebrow_left_5",
+        "face_nose_1",
+        "face_nose_2",
+        "face_nose_3",
+        "face_nose_4",
+        "face_nose_5",
+        "face_nose_6",
+        "face_nose_7",
+        "face_nose_8",
+        "face_nose_9",
+        "face_eye_right_1",
+        "face_eye_right_2",
+        "face_eye_right_3",
+        "face_eye_right_4",
+        "face_eye_right_5",
+        "face_eye_right_6",
+        "face_eye_left_1",
+        "face_eye_left_2",
+        "face_eye_left_3",
+        "face_eye_left_4",
+        "face_eye_left_5",
+        "face_eye_left_6",
+        "face_mouth_1",
+        "face_mouth_2",
+        "face_mouth_3",
+        "face_mouth_4",
+        "face_mouth_5",
+        "face_mouth_6",
+        "face_mouth_7",
+        "face_mouth_8",
+        "face_mouth_9",
+        "face_mouth_10",
+        "face_mouth_11",
+        "face_mouth_12",
+        "face_mouth_13",
+        "face_mouth_14",
+        "face_mouth_15",
+        "face_mouth_16",
+        "face_mouth_17",
+        "face_mouth_18",
+        "face_mouth_19",
+        "face_mouth_20",
+        // hand
+        "hand_wrist_left",
+        "hand_finger_thumb_left_1",
+        "hand_finger_thumb_left_2",
+        "hand_finger_thumb_left_3",
+        "hand_finger_thumb_left_4",
+        "hand_finger_index_left_1",
+        "hand_finger_index_left_2",
+        "hand_finger_index_left_3",
+        "hand_finger_index_left_4",
+        "hand_finger_middle_left_1",
+        "hand_finger_middle_left_2",
+        "hand_finger_middle_left_3",
+        "hand_finger_middle_left_4",
+        "hand_finger_ring_left_1",
+        "hand_finger_ring_left_2",
+        "hand_finger_ring_left_3",
+        "hand_finger_ring_left_4",
+        "hand_finger_pinky_left_1",
+        "hand_finger_pinky_left_2",
+        "hand_finger_pinky_left_3",
+        "hand_finger_pinky_left_4",
+        "hand_wrist_right",
+        "hand_finger_thumb_right_1",
+        "hand_finger_thumb_right_2",
+        "hand_finger_thumb_right_3",
+        "hand_finger_thumb_right_4",
+        "hand_finger_index_right_1",
+        "hand_finger_index_right_2",
+        "hand_finger_index_right_3",
+        "hand_finger_index_right_4",
+        "hand_finger_middle_right_1",
+        "hand_finger_middle_right_2",
+        "hand_finger_middle_right_3",
+        "hand_finger_middle_right_4",
+        "hand_finger_ring_right_1",
+        "hand_finger_ring_right_2",
+        "hand_finger_ring_right_3",
+        "hand_finger_ring_right_4",
+        "hand_finger_pinky_right_1",
+        "hand_finger_pinky_right_2",
+        "hand_finger_pinky_right_3",
+        "hand_finger_pinky_right_4",
+        // extras
+        "hip_middle",
+        "shoulder_middle",
+        "head",
+    };
+
+    // =============================================================================================
+    // =============================================================================================
+
+    [[maybe_unused]] static bool use_whole_body()
+    {
+        for (const auto &pair : whole_body)
+        {
+            if (pair.second)
+            {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    // =============================================================================================
+
+    cv::Mat bayer2rgb(const cv::Mat &bayer)
+    {
+        cv::Mat rgb;
+        cv::cvtColor(bayer, rgb, cv::COLOR_BayerBG2RGB);
+        return rgb;
+    }
+
+    cv::Mat rgb2bayer(const cv::Mat &img)
+    {
+        CV_Assert(img.type() == CV_8UC3);
+        cv::Mat bayer(img.rows, img.cols, CV_8UC1);
+
+        for (int r = 0; r < img.rows; ++r)
+        {
+            const uchar *imgData = img.ptr<uchar>(r);
+            uchar *bayerRowPtr = bayer.ptr<uchar>(r);
+
+            for (int c = 0; c < img.cols; ++c)
+            {
+                int pixelIndex = 3 * c;
+
+                // Use faster bit operation instead of modulo+if
+                // Even row, even col => R = 0
+                // Even row, odd col => G = 1
+                // Odd row, even col => G = 1
+                // Odd row, odd col => B = 2
+                int row_mod = r & 1;
+                int col_mod = c & 1;
+                int component = row_mod + col_mod;
+
+                bayerRowPtr[c] = imgData[pixelIndex + component];
+            }
+        }
+
+        return bayer;
+    }
+
+    // =============================================================================================
+
+    inline int find_index(const std::vector<std::string> &vec, const std::string &key)
+    {
+        auto it = std::find(vec.begin(), vec.end(), key);
+        if (it == vec.end())
+        {
+            throw std::runtime_error("Cannot find \"" + key + "\" in joint_names.");
+        }
+        return static_cast<int>(std::distance(vec.begin(), it));
+    }
+
+    std::vector<std::vector<std::vector<std::array<float, 3>>>> update_keypoints(
+        const std::vector<std::vector<std::vector<std::array<float, 3>>>> &poses_2d,
+        const std::vector<std::string> &joint_names)
+    {
+        std::vector<std::vector<std::vector<std::array<float, 3>>>> new_views;
+        new_views.reserve(poses_2d.size());
+
+        for (const auto &view : poses_2d)
+        {
+            // "view" is a list of bodies => each body is Nx3
+            std::vector<std::vector<std::array<float, 3>>> new_bodies;
+            new_bodies.reserve(view.size());
+
+            for (const auto &body : view)
+            {
+                // 1) Copy first 17 keypoints
+                std::vector<std::array<float, 3>> new_body;
+                new_body.insert(new_body.end(), body.begin(), body.begin() + 17);
+
+                // 2) Optionally append extra keypoints
+                if (whole_body.at("foots"))
+                {
+                    new_body.insert(new_body.end(), body.begin() + 17, body.begin() + 23);
+                }
+                if (whole_body.at("face"))
+                {
+                    new_body.insert(new_body.end(), body.begin() + 23, body.begin() + 91);
+                }
+                if (whole_body.at("hands"))
+                {
+                    new_body.insert(new_body.end(), body.begin() + 91, body.end());
+                }
+
+                // 3) Compute mid_hip
+                int hlid = find_index(joint_names, "hip_left");
+                int hrid = find_index(joint_names, "hip_right");
+                float mid_hip_x = 0.5 * (new_body[hlid][0] + new_body[hrid][0]);
+                float mid_hip_y = 0.5 * (new_body[hlid][1] + new_body[hrid][1]);
+                float mid_hip_c = std::min(new_body[hlid][2], new_body[hrid][2]);
+                new_body.push_back({mid_hip_x, mid_hip_y, mid_hip_c});
+
+                // 4) Compute mid_shoulder
+                int slid = find_index(joint_names, "shoulder_left");
+                int srid = find_index(joint_names, "shoulder_right");
+                float mid_shoulder_x = 0.5 * (new_body[slid][0] + new_body[srid][0]);
+                float mid_shoulder_y = 0.5 * (new_body[slid][1] + new_body[srid][1]);
+                float mid_shoulder_c = std::min(new_body[slid][2], new_body[srid][2]);
+                new_body.push_back({mid_shoulder_x, mid_shoulder_y, mid_shoulder_c});
+
+                // 5) Compute head
+                int elid = find_index(joint_names, "ear_left");
+                int erid = find_index(joint_names, "ear_right");
+                float head_x = 0.5 * (new_body[elid][0] + new_body[erid][0]);
+                float head_y = 0.5 * (new_body[elid][1] + new_body[erid][1]);
+                float head_c = std::min(new_body[elid][2], new_body[erid][2]);
+                new_body.push_back({head_x, head_y, head_c});
+
+                // Add this updated body into new_bodies
+                new_bodies.push_back(new_body);
+            }
+
+            // Add all updated bodies for this view
+            new_views.push_back(new_bodies);
+        }
+
+        return new_views;
+    }
+}
--- a/ros/rpt2D_wrapper_cpp/src/utils_2d_pose.hpp
+++ b/ros/rpt2D_wrapper_cpp/src/utils_2d_pose.hpp
--- a/ros/rpt2D_wrapper_cpp/tests/.gitignore
+++ b/ros/rpt2D_wrapper_cpp/tests/.gitignore
@ -0,0 +1 @@
+my_app
--- a/ros/rpt2D_wrapper_cpp/tests/my_app.cpp
+++ b/ros/rpt2D_wrapper_cpp/tests/my_app.cpp
@ -0,0 +1,96 @@
+#include <filesystem>
+#include <vector>
+#include <string>
+#include <memory>
+
+#include <opencv2/opencv.hpp>
+
+#include "../src/utils_2d_pose.hpp"
+
+// =================================================================================================
+// =================================================================================================
+
+int main(int argc, char **argv)
+{
+    using namespace utils_2d_pose;
+
+    std::string base_path = "/RapidPoseTriangulation/extras/mmdeploy/exports/";
+    std::string model_path1 = base_path + "rtmdet-nano_1x320x320x3_fp16_extra-steps.onnx";
+    std::string model_path2 = base_path + "rtmpose-m_1x384x288x3_fp16_extra-steps.onnx";
+
+    std::vector<std::string> img_paths = {
+        "/RapidPoseTriangulation/data/h1/54138969-img_003201.jpg",
+        "/RapidPoseTriangulation/data/h1/55011271-img_003201.jpg",
+        "/RapidPoseTriangulation/data/h1/58860488-img_003201.jpg",
+        "/RapidPoseTriangulation/data/h1/60457274-img_003201.jpg",
+    };
+
+    // {
+    //     std::cout << "\nTesting RTMDet and RTMPose" << std::endl;
+    //     RTMDet model1(model_path1, 0.3, 0.1 * 0.1, 30);
+    //     RTMPose model2(model_path2, 30);
+    //     for (size_t i = 0; i < img_paths.size(); i++)
+    //     {
+    //         cv::Mat img = cv::imread(img_paths[i]);
+    //         cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
+
+    //         auto outputs1 = model1.call(img);
+    //         std::cout << "Model1 outputs: " << outputs1[0][0] << " " << outputs1[0][1] << " "
+    //                   << outputs1[0][2] << " " << outputs1[0][3] << " " << outputs1[0][4] << " "
+    //                   << std::endl;
+
+    //         for (size_t j = 0; j < outputs1.size(); j++)
+    //         {
+    //             std::vector<std::array<float, 5>> bboxes = {outputs1[j]};
+    //             auto outputs2 = model2.call(img, bboxes);
+    //             std::cout << "Model2 outputs: " << outputs2[0][0][0] << " "
+    //                       << outputs2[0][0][1] << " " << outputs2[0][0][2] << " " << std::endl;
+    //         }
+    //     }
+    // }
+
+    // {
+    //     std::cout << "\nTesting TopDown" << std::endl;
+    //     TopDown model3(model_path1, model_path2, 0.3, 0.1 * 0.1, false, 30);
+    //     for (size_t i = 0; i < img_paths.size(); i++)
+    //     {
+    //         cv::Mat img = cv::imread(img_paths[i]);
+    //         cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
+
+    //         auto outputs3 = model3.predict(img);
+    //         std::cout << "Model3 outputs: " << outputs3[0][0][0] << " "
+    //                   << outputs3[0][0][1] << " " << outputs3[0][0][2] << " " << std::endl;
+    //     }
+    // }
+
+    {
+        std::cout << "\nTesting PosePredictor 1" << std::endl;
+        PosePredictor model4(false, 0.3, 0.1 * 0.1, false);
+        std::vector<cv::Mat> images;
+        for (size_t i = 0; i < img_paths.size(); i++)
+        {
+            cv::Mat img = cv::imread(img_paths[i]);
+            cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
+            images.push_back(img);
+        }
+        auto outputs4 = model4.predict(images);
+        std::cout << "Model4 outputs: " << outputs4[0][0][0][0] << " "
+                  << outputs4[0][0][0][1] << " " << outputs4[0][0][0][2] << " " << std::endl;
+    }
+
+    {
+        std::cout << "\nTesting PosePredictor 2" << std::endl;
+        PosePredictor model5(false, 0.3, 0.1 * 0.1, true);
+        std::vector<cv::Mat> images;
+        for (size_t i = 0; i < img_paths.size(); i++)
+        {
+            cv::Mat img = cv::imread(img_paths[i]);
+            cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
+            images.push_back(img);
+        }
+        auto outputs5 = model5.predict(images);
+        std::cout << "Model5 outputs: " << outputs5[0][0][0][0] << " "
+                  << outputs5[0][0][0][1] << " " << outputs5[0][0][0][2] << " " << std::endl;
+    }
+    return 0;
+}
--- a/ros/rpt2D_wrapper_py/package.xml
+++ b/ros/rpt2D_wrapper_py/package.xml
@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rpt2D_wrapper_py</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
--- a/ros/rpt2D_wrapper_py/resource/rpt2D_wrapper_py
+++ b/ros/rpt2D_wrapper_py/resource/rpt2D_wrapper_py
--- a/ros/rpt2D_wrapper_py/rpt2D_wrapper_py/init.py
+++ b/ros/rpt2D_wrapper_py/rpt2D_wrapper_py/init.py
--- a/ros/rpt2D_wrapper_py/rpt2D_wrapper_py/rpt2D_wrapper.py
+++ b/ros/rpt2D_wrapper_py/rpt2D_wrapper_py/rpt2D_wrapper.py
@ -0,0 +1,196 @@
+import json
+import os
+import sys
+import threading
+import time
+
+import numpy as np
+import rclpy
+from cv_bridge import CvBridge
+from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+filepath = os.path.dirname(os.path.realpath(__file__)) + "/"
+sys.path.append(filepath + "../../../scripts/")
+import test_triangulate
+import utils_2d_pose
+
+# ==================================================================================================
+
+bridge = CvBridge()
+node = None
+publisher_pose = None
+
+cam_id = "camera01"
+img_input_topic = "/" + cam_id + "/pylon_ros2_camera_node/image_raw"
+pose_out_topic = "/poses/" + cam_id
+
+last_input_image = None
+last_input_time = 0.0
+kpt_model = None
+joint_names_2d = test_triangulate.joint_names_2d
+
+lock = threading.Lock()
+stop_flag = False
+
+# Model config
+min_bbox_score = 0.4
+min_bbox_area = 0.1 * 0.1
+batch_poses = True
+
+
+# ==================================================================================================
+
+
+def callback_images(image_data):
+    global last_input_image, last_input_time, lock
+
+    # Convert into cv images from image string
+    if image_data.encoding == "bayer_rggb8":
+        bayer_image = bridge.imgmsg_to_cv2(image_data, "bayer_rggb8")
+    elif image_data.encoding == "mono8":
+        bayer_image = bridge.imgmsg_to_cv2(image_data, "mono8")
+    elif image_data.encoding == "rgb8":
+        color_image = bridge.imgmsg_to_cv2(image_data, "rgb8")
+        bayer_image = test_triangulate.rgb2bayer(color_image)
+    else:
+        raise ValueError("Unknown image encoding:", image_data.encoding)
+
+    time_stamp = image_data.header.stamp.sec + image_data.header.stamp.nanosec / 1.0e9
+
+    with lock:
+        last_input_image = bayer_image
+        last_input_time = time_stamp
+
+
+# ==================================================================================================
+
+
+def callback_model():
+    global last_input_image, last_input_time, kpt_model, lock
+
+    ptime = time.time()
+    if last_input_time == 0.0:
+        time.sleep(0.0001)
+        return
+
+    # Collect inputs
+    images_2d = []
+    timestamps = []
+    with lock:
+        img = last_input_image
+        ts = last_input_time
+        images_2d.append(img)
+        timestamps.append(ts)
+        last_input_image = None
+        last_input_time = 0.0
+
+    # Predict 2D poses
+    images_2d = [test_triangulate.bayer2rgb(img) for img in images_2d]
+    poses_2d = utils_2d_pose.get_2d_pose(kpt_model, images_2d)
+    poses_2d = test_triangulate.update_keypoints(poses_2d, joint_names_2d)
+    poses_2d = poses_2d[0]
+
+    # Drop persons with no joints
+    poses_2d = np.asarray(poses_2d)
+    mask = np.sum(poses_2d[..., 2], axis=1) > 0
+    poses_2d = poses_2d[mask]
+
+    # Round poses
+    poses2D = [np.array(p).round(3).tolist() for p in poses_2d]
+
+    # Publish poses
+    ts_pose = time.time()
+    poses = {
+        "bodies2D": poses2D,
+        "joints": joint_names_2d,
+        "num_persons": len(poses2D),
+        "timestamps": {
+            "image": timestamps[0],
+            "pose": ts_pose,
+            "z-images-pose": ts_pose - timestamps[0],
+        },
+    }
+    publish(poses)
+
+    msg = "Detected persons: {} - Prediction time: {:.4f}s"
+    print(msg.format(poses["num_persons"], time.time() - ptime))
+
+
+# ==================================================================================================
+
+
+def callback_wrapper():
+    global stop_flag
+    while not stop_flag:
+        callback_model()
+        time.sleep(0.001)
+
+
+# ==================================================================================================
+
+
+def publish(data):
+    # Publish json data
+    msg = String()
+    msg.data = json.dumps(data)
+    publisher_pose.publish(msg)
+
+
+# ==================================================================================================
+
+
+def main():
+    global node, publisher_pose, kpt_model, stop_flag
+
+    # Start node
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("rpt2D_wrapper")
+
+    # Quality of service settings
+    qos_profile = QoSProfile(
+        reliability=QoSReliabilityPolicy.RELIABLE,
+        history=QoSHistoryPolicy.KEEP_LAST,
+        depth=1,
+    )
+
+    # Create subscribers
+    _ = node.create_subscription(
+        Image,
+        img_input_topic,
+        callback_images,
+        qos_profile,
+    )
+
+    # Create publishers
+    publisher_pose = node.create_publisher(String, pose_out_topic, qos_profile)
+
+    # Load 2D pose model
+    whole_body = test_triangulate.whole_body
+    if any((whole_body[k] for k in whole_body)):
+        kpt_model = utils_2d_pose.load_wb_model(
+            min_bbox_score, min_bbox_area, batch_poses
+        )
+    else:
+        kpt_model = utils_2d_pose.load_model(min_bbox_score, min_bbox_area, batch_poses)
+
+    node.get_logger().info("Finished initialization of pose estimator")
+
+    # Start prediction thread
+    p1 = threading.Thread(target=callback_wrapper)
+    p1.start()
+
+    # Run ros update thread
+    rclpy.spin(node)
+
+    stop_flag = True
+    p1.join()
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+# ==================================================================================================
+
+if __name__ == "__main__":
+    main()
--- a/ros/rpt2D_wrapper_py/setup.cfg
+++ b/ros/rpt2D_wrapper_py/setup.cfg
@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rpt2D_wrapper_py
+[install]
+install_scripts=$base/lib/rpt2D_wrapper_py
--- a/ros/rpt2D_wrapper_py/setup.py
+++ b/ros/rpt2D_wrapper_py/setup.py
@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = "rpt2D_wrapper_py"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="root",
+    maintainer_email="root@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": ["rpt2D_wrapper = rpt2D_wrapper_py.rpt2D_wrapper:main"],
+    },
+)
--- a/ros/rpt2D_wrapper_py/test/test_copyright.py
+++ b/ros/rpt2D_wrapper_py/test/test_copyright.py
@ -0,0 +1,27 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+from ament_copyright.main import main
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(
+    reason="No copyright header has been placed in the generated source file."
+)
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"
--- a/ros/rpt2D_wrapper_py/test/test_flake8.py
+++ b/ros/rpt2D_wrapper_py/test/test_flake8.py
@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+from ament_flake8.main import main_with_errors
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
--- a/ros/rpt2D_wrapper_py/test/test_pep257.py
+++ b/ros/rpt2D_wrapper_py/test/test_pep257.py
@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import pytest
+from ament_pep257.main import main
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found code style errors / warnings"
--- a/scripts/test_skelda_dataset.py
+++ b/scripts/test_skelda_dataset.py
@ -61,7 +61,7 @@ datasets = {
    "human36m": {
        "path": "/datasets/human36m/skelda/pose_test.json",
        "take_interval": 5,
-        "min_match_score": 0.94,
+        "min_match_score": 0.95,
        "min_group_size": 1,
        "min_bbox_score": 0.4,
        "min_bbox_area": 0.1 * 0.1,
@ -73,6 +73,7 @@ datasets = {
        # "cams": ["00_03", "00_06", "00_12"],
        # "cams": ["00_03", "00_06", "00_12", "00_13", "00_23", "00_15", "00_10", "00_21", "00_09", "00_01"],
        "take_interval": 3,
+        "min_match_score": 0.95,
        "use_scenes": ["160906_pizza1", "160422_haggling1", "160906_ian5"],
        "min_group_size": 1,
        # "min_group_size": 4,
@ -88,6 +89,7 @@ datasets = {
    "campus": {
        "path": "/datasets/campus/skelda/test.json",
        "take_interval": 1,
+        "min_match_score": 0.90,
        "min_bbox_score": 0.5,
    },
    "shelf": {
@ -109,6 +111,7 @@ datasets = {
    "tsinghua": {
        "path": "/datasets/tsinghua/skelda/test.json",
        "take_interval": 3,
+        "min_match_score": 0.95,
        "min_group_size": 2,
    },
    "human36m_wb": {
@ -122,7 +125,7 @@ datasets = {
        "take_interval": 2,
        "subset": "tagging",
        "min_group_size": 2,
-        "min_bbox_score": 0.25,
+        "min_bbox_score": 0.2,
        "min_bbox_area": 0.05 * 0.05,
    },
    "egohumans_legoassemble": {
@ -343,19 +346,32 @@ def main():
    # Print a dataset sample for debugging
    print(labels[0])

+    print("\nPrefetching images ...")
+    for label in tqdm.tqdm(labels):
+        # If the images are stored on a HDD, it sometimes takes a while to load them
+        # Prefetching them results in more stable timings of the following steps
+        # To prevent memory overflow, the code only loads the images, but does not store them
+        try:
+            for i in range(len(label["imgpaths"])):
+                imgpath = label["imgpaths"][i]
+                img = test_triangulate.load_image(imgpath)
+        except cv2.error:
+            print("One of the paths not found:", label["imgpaths"])
+            continue
+    time.sleep(3)
+
    print("\nCalculating 2D predictions ...")
    all_poses_2d = []
    times = []
    for label in tqdm.tqdm(labels):
        images_2d = []

-        try:
        start = time.time()
+        try:
            for i in range(len(label["imgpaths"])):
                imgpath = label["imgpaths"][i]
                img = test_triangulate.load_image(imgpath)
                images_2d.append(img)
-            time_imgs = time.time() - start
        except cv2.error:
            print("One of the paths not found:", label["imgpaths"])
            continue
@ -373,7 +389,16 @@ def main():
                    cam["K"][0][2] = cam["K"][0][2] * (1000 / ishape[1])
                    images_2d[i] = cv2.resize(img, (1000, 1000))

+        # Convert image format to Bayer encoding to simulate real camera input
+        # This also resulted in notably better MPJPE results in most cases, presumbly since the
+        # demosaicing algorithm from OpenCV is better than the default one from the cameras
+        for i in range(len(images_2d)):
+            images_2d[i] = test_triangulate.rgb2bayer(images_2d[i])
+        time_imgs = time.time() - start
+
        start = time.time()
+        for i in range(len(images_2d)):
+            images_2d[i] = test_triangulate.bayer2rgb(images_2d[i])
        poses_2d = utils_2d_pose.get_2d_pose(kpt_model, images_2d)
        poses_2d = test_triangulate.update_keypoints(poses_2d, joint_names_2d)
        time_2d = time.time() - start
--- a/scripts/test_triangulate.py
+++ b/scripts/test_triangulate.py
@ -227,6 +227,23 @@ def load_image(path: str):
 # ==================================================================================================


+def rgb2bayer(img):
+    bayer = np.zeros((img.shape[0], img.shape[1]), dtype=img.dtype)
+    bayer[0::2, 0::2] = img[0::2, 0::2, 0]
+    bayer[0::2, 1::2] = img[0::2, 1::2, 1]
+    bayer[1::2, 0::2] = img[1::2, 0::2, 1]
+    bayer[1::2, 1::2] = img[1::2, 1::2, 2]
+    return bayer
+
+
+def bayer2rgb(bayer):
+    img = cv2.cvtColor(bayer, cv2.COLOR_BayerBG2RGB)
+    return img
+
+
+# ==================================================================================================
+
+
 def update_keypoints(poses_2d: list, joint_names: List[str]) -> list:
    new_views = []
    for view in poses_2d:
@ -236,11 +253,11 @@ def update_keypoints(poses_2d: list, joint_names: List[str]) -> list:

            new_body = body[:17]
            if whole_body["foots"]:
-                new_body.extend(body[17:22])
+                new_body.extend(body[17:23])
            if whole_body["face"]:
-                new_body.extend(body[22:90])
+                new_body.extend(body[23:91])
            if whole_body["hands"]:
-                new_body.extend(body[90:])
+                new_body.extend(body[91:])
            body = new_body

            hlid = joint_names.index("hip_left")
@ -314,6 +331,8 @@ def main():
        for i in range(len(sample["cameras_color"])):
            imgpath = sample["imgpaths_color"][i]
            img = load_image(imgpath)
+            img = rgb2bayer(img)
+            img = bayer2rgb(img)
            images_2d.append(img)

        # Get 2D poses
--- a/scripts/utils_2d_pose.py
+++ b/scripts/utils_2d_pose.py
@ -189,9 +189,15 @@ class BoxCrop:
        self.fill_value = fill_value

    def calc_params(self, ishape, bbox):
-        start_x, start_y, end_x, end_y = bbox[0], bbox[1], bbox[2], bbox[3]
+        img_h, img_w = ishape[:2]
        target_h, target_w = self.target_size

+        # Round the bounding box coordinates
+        start_x = math.floor(bbox[0])
+        start_y = math.floor(bbox[1])
+        end_x = math.ceil(bbox[2])
+        end_y = math.ceil(bbox[3])
+
        # Calculate original bounding box center
        center_x = (start_x + end_x) / 2.0
        center_y = (start_y + end_y) / 2.0
@ -231,8 +237,8 @@ class BoxCrop:
        # Define the new box coordinates
        new_start_x = max(0, start_x)
        new_start_y = max(0, start_y)
-        new_end_x = min(ishape[1] - 1, end_x)
-        new_end_y = min(ishape[0] - 1, end_y)
+        new_end_x = min(img_w - 1, end_x)
+        new_end_y = min(img_h - 1, end_y)
        new_box = [new_start_x, new_start_y, new_end_x, new_end_y]

        # Calculate resized crop size
@ -332,9 +338,8 @@ class RTMDet(BaseModel):

    def postprocess(self, result: List[np.ndarray], image: np.ndarray):
        boxes = np.squeeze(result[0], axis=0)
-        classes = np.squeeze(result[1], axis=0)

-        human_class = classes[:] == 0
+        human_class = boxes[:, 5] == 0
        boxes = boxes[human_class]

        keep = boxes[:, 4] > self.conf_threshold
@ -344,7 +349,6 @@ class RTMDet(BaseModel):
            return np.array([])

        # Drop boxes with too small area
-        boxes = boxes.astype(np.float32)
        areas = (boxes[:, 2] - boxes[:, 0]) * (boxes[:, 3] - boxes[:, 1])
        keep = areas >= self.min_area
        boxes = boxes[keep]
@ -386,10 +390,7 @@ class RTMPose(BaseModel):
    def preprocess(self, image: np.ndarray, bboxes: np.ndarray):
        cutouts = []
        for i in range(len(bboxes)):
-            bbox = np.asarray(bboxes[i])[0:4]
-            bbox += np.array([-0.5, -0.5, 0.5 - 1e-8, 0.5 - 1e-8])
-            bbox = bbox.round().astype(np.int32)
-            region = self.boxcrop.crop_resize_box(image, bbox)
+            region = self.boxcrop.crop_resize_box(image, bboxes[i])
            tensor = np.asarray(region).astype(self.input_types[0], copy=False)
            cutouts.append(tensor)

@ -406,10 +407,7 @@ class RTMPose(BaseModel):
    ):
        kpts = []
        for i in range(len(bboxes)):
-            scores = np.clip(result[1][i], 0, 1)
-            kp = np.concatenate(
-                [result[0][i], np.expand_dims(scores, axis=-1)], axis=-1
-            )
+            kp = result[0][i]

            paddings, scale, bbox, _ = self.boxcrop.calc_params(image.shape, bboxes[i])
            kp[:, 0] -= paddings[0]