SlendyBoy/Robovision

Brique ROS2 de vision permettant d'identifier et caractériser des objets et des personnes à partir d'une caméra 3D. Sujet de projet de robotique à CPE Lyon dans le cadre de la Robocup.

Projet robotique

Sujet 3 - Brique vision

Auteur : Tanguy FROUIN 5IRC

État de l'Art

"Awesome lists" - listes d'articles de recherche

• Une liste d'articles et d'ensembles de données sur l'analyse des nuages de points (traitement)
• Une collection de projets publics impressionnants de détection d'objets YOLO
• Articles, codes et ensembles de données sur l'apprentissage profond pour la détection d'objets en 3D

Point Clouds

• NVIDIA Developer Blog sur les nuages de points et ROS 2
• Stereolabs - Depth Sensing avec ROS2
• Exemples Python pour Intel RealSense
• Traitement de nuages de points stéréoscopiques
• PPTK - Python Point Processing Toolkit
• Python-PCL (Point Cloud Library)
• Perception PCL sur ROS
• PCL ROS sur ROS Humble
• Importation de nuages de points avec Python API
• Vidéo YouTube sur la classification de nuages de points 3D - PointNet
• Tutoriel sur la pipeline d'image ROS2
• Traitement de nuages de points pour la détection d'objets 3D
• De la carte de profondeur au nuage de points
• Thèse sur la perception d'objets YOLO 3D
• Open3D pour les images RGBD
• Open3D pour les nuages de points
• Utilisation de PointNet pour la vision par ordinateur 3D

Segmentation

• Documentation OpenCV K-Means pour la segmentation d'images
• Article K-Means pour la segmentation d'images avec OpenCV en Python
• Site Google K-Means sur image avec OpenCV
• Introduction à la segmentation d'images avec K-Means
• Implémentation de la segmentation d'instance avec YOLOv8

Détection visages et attributs

• Face recognition : détection de visages
• DeepFace : détection d'attributs
• Modèle dlib
• Insightface analyse 2D et 3D de visages

Mediapipe

• Solutions Mediapipe de Google
• Exemples Mediapipe de Google
• Guide des solutions Mediapipe

OpenPose

• OpenPose par CMU Perceptual Computing Lab
• Documentation OpenPose
• Estimation de pose avec TensorFlow
• Estimation de pose lightweight avec PyTorch
• OpenPose avec ROS par Firephinx
• ROS OpenPose par Ravijo

Ultralytics

• Issues Ultralytics sur GitHub
• Documentation Ultralytics

Intel RealSense

• Librealsense par Intel
• RealSense ROS par Intel
• Documentation Python pour Intel RealSense
• Ressources pour développeurs Intel RealSense

Optimisation

• CUDA
• CuPy - NumPy-like API pour GPU

Liste des fonctionnalités

• Fonctionnement sur plusieurs caméras 3D

• Noeud pointcloud_proc :

  • Récupération du nuage de points 3D depuis la caméra
  • Projection des points 3D en 2D sur l'image RGB
  • Création d'une image de profondeur
    • Publication données brutes sur le topic /robovision/depth/raw
    • Publication visualisation sur le topic /robovision/depth/color

• Noeud vision_obj_pers :

  • Inférence du modèle de segmentation YOLOv8 :
    • Traitement pour chaque objet détecté
      • Affichage des bounding boxes et informations de l'objet
      • Calcul et affichage de la distance objet-caméra
      • Projection des pixels (centre bbox) dans le monde réel en mètres
      • Publication des TFs (x,y,z)
      • Taxonomie
  • Inférence du modèle de pose de YOLOv8 :
    • Analyse des positions des personnes (assis, debout, couché)
    • Affichage du squelette et de la position dans la bounding box

• Noeud face_reco_analysis :

  • Détection des visages
  • Détection des attributs (âge, genre, émotion, ethnie)

Représentation des noeuds ROS2

graph TD
    Camera3D[Caméra 3D] -->|/camera/camera/depth/color/points| pointcloud_proc[pointcloud_proc]
    Camera3D -->|/camera/camera/color/image_raw| pointcloud_proc
    Camera3D -->|/camera/camera/color/image_raw| face_reco_analysis[face_reco_analysis]
    pointcloud_proc -->|/robovision/depth/raw| vision_obj_pers[vision_obj_pers]
    pointcloud_proc -->|/robovision/depth/color| rviz
    Camera3D -->|/camera/camera/color/image_raw| vision_obj_pers
    vision_obj_pers -->|TFs| rviz[rviz]

Loading

Description de l'algo

sequenceDiagram
participant Camera3D as Caméra 3D
participant pointcloud_proc as pointcloud_proc
participant vision_obj_pers as vision_obj_pers
participant YOLOv8 as Modèle YOLOv8
participant face_reco_analysis as face_reco_analysis
participant dlib as Modèle dlib
participant deepface as Modèles DeepFace
participant rviz as rviz

Camera3D-->>pointcloud_proc: /camera/camera/depth/color/points
Camera3D-->>pointcloud_proc: /camera/camera/color/image_raw
pointcloud_proc->>pointcloud_proc: Projection points 3D à 2D
pointcloud_proc->>pointcloud_proc: création et publication image de profondeur
pointcloud_proc-->>vision_obj_pers: /robovision/depth/raw
pointcloud_proc-->>rviz: /robovision/depth/color
Camera3D-->>vision_obj_pers: /camera/camera/color/image_raw
Camera3D-->>face_reco_analysis: /camera/camera/color/image_raw

loop Pour chaque frame RGB

vision_obj_pers->>vision_obj_pers: Correction distortion image RGB
vision_obj_pers->>vision_obj_pers: Création seg_frame
vision_obj_pers->>YOLOv8: Inférence segmentation
YOLOv8->>vision_obj_pers: Objets détectés
vision_obj_pers->>vision_obj_pers: Calcul profondeur obj
vision_obj_pers->>vision_obj_pers: Projection pixels RGB à points 3D en mètres
vision_obj_pers-->>rviz: Publication TF

vision_obj_pers->>vision_obj_pers: Création pose_frame
vision_obj_pers->>YOLOv8: Inférence Pose
YOLOv8->>vision_obj_pers: Keypoints et membres
vision_obj_pers->>vision_obj_pers: Calcul position

vision_obj_pers->>vision_obj_pers: Superposition seg_frame et pose_frame

face_reco_analysis->>dlib: Inférence détection visages
dlib->>face_reco_analysis: Emplacements visages
end

loop Toutes les 100 frames RGB
face_reco_analysis->>deepface: Emplacements visages
deepface->>face_reco_analysis: attributs
face_reco_analysis->>face_reco_analysis: MAJ attributs
end

Loading

La caméra envoie le nuage de points 3D :

Visualisation du pointcloud envoyé par la caméra

Projection des points 3D du nuage en 2D à l'aides des intrinsèques et extrinsèques de la caméra :

Visualisation des points 3D projetés en 2D sur l'image RGB

Après le calcul de projection, on crée l'image de profondeur qui sera publiée et utilisée pour les calculs de distance :

Visualisation de l'image de profondeur brut

Création de la même image mais en RGB, qui sera elle aussi publiée :

Visualisation de l'image de profondeur RGB

Le calcul de la profondeur est basé sur les pixels des masques de segmentation des objets. On fait ensuite la médiane de la profondeur des points 3D correspondant à ces pixels. Voici par exemple les masques affichés :

Visualisation détection et segmentation objets, distance, pose et position personne

Par soucis de performance, on n'affiche pas les masques de segmentation.
Voici l'image finale :

Visualisation détection objets, distance, pose et position personne

On récupère le centre des bounding boxes des objets/personnes (x,y en pixels) puis le pixel est projeté dans le référentiel du monde réel en mètre à l'aide des intrinsèques et extrinsèques de la caméra pour ensuite publier le TF correspondant (x,y,z; z étant la distance calculée précédemment) :

Visualisation des TFs

Concernant la reconnaissance de visage, la détection se fait toutes les frames alors que la détection d'attributs se fait toutes les 100 frames

Détection visage et attributs

Pour voir les performances de l'algo :

nvidia-smi

Ressources utilisées :

• 170 MiB pour pointcloud_proc
• 650 MiB pour vision_obj_pers
• 6260 MiB pour face_reco_analysis

+---------------------------------------------------------------------------------------+
| NVIDIA-SMI 535.146.02             Driver Version: 535.146.02   CUDA Version: 12.2     |
|-----------------------------------------+----------------------+----------------------+
| GPU  Name                 Persistence-M | Bus-Id        Disp.A | Volatile Uncorr. ECC |
| Fan  Temp   Perf          Pwr:Usage/Cap |         Memory-Usage | GPU-Util  Compute M. |
|                                         |                      |               MIG M. |
|=========================================+======================+======================|
|   0  NVIDIA GeForce RTX 2070 ...    Off | 00000000:01:00.0 Off |                  N/A |
| N/A   75C    P0              80W /  80W |   7094MiB /  8192MiB |     56%      Default |
|                                         |                      |                  N/A |
+-----------------------------------------+----------------------+----------------------+
                                                                                         
+---------------------------------------------------------------------------------------+
| Processes:                                                                            |
|  GPU   GI   CI        PID   Type   Process name                            GPU Memory |
|        ID   ID                                                             Usage      |
|=======================================================================================|
|    0   N/A  N/A      2146      G   /usr/lib/xorg/Xorg                            4MiB |
|    0   N/A  N/A    314080      C   /usr/bin/python3                            172MiB |
|    0   N/A  N/A    314082      C   /usr/bin/python3                            654MiB |
|    0   N/A  N/A    314084      C   /usr/bin/python3                           6260MiB |
+---------------------------------------------------------------------------------------+

Performances

Les maths derrière

Transformation 2D en 3D

Matrice intrinsèque caméra RGB :

$$ \text{CameraMatrix} = \begin{bmatrix} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \end{bmatrix} $$

Dans cette matrice :

• $fx$ : longueur focale horizontale (px)
• $fy$ : longueur focale verticale (px)
• $cx$ : coordonnée horizontale du point principal (centre optique) (px)
• $cy$ : coordonnée verticale du point principal (centre optique) (px)

Cette matrice est utilisée pour effectuer la transformation des coordonnées 3D du monde réel en coordonnées 2D sur le plan de l'image de la caméra.

Point 2D $[x,y]$ en point 3D $[X, Y, Z]$ :

$X = (x - c_x) * d / fx$
$Y = (y - c_y) * d / fy$
$Z = d$

Appliquer les transformations extrinsèques aux points 3D calculés :

Matrice de Rotation et Vecteur de Translation : représentent la transformation entre les systèmes de coordonnées de la caméra de profondeur et de la caméra RGB.

Transformation des Points 3D : Chaque point 3D calculé doit être transformé en utilisant $R$ et $T$. Pour un point 3D $[X,Y,Z]$, la transformation se fait comme suit :

$$ \begin{bmatrix} X' \ Y' \ Z' \end{bmatrix} = R \times \begin{bmatrix} X \ Y \ Z \end{bmatrix} + T $$

Où X′, Y′, et Z′ sont les coordonnées transformées dans le système de coordonnées du monde réel.

Transformation 3D en 2D

Pour chaque point 3D $[X, Y, Z]$ dans le nuage de points :

Appliquer les extrinsèques (rotation et translation) :

$$ \begin{bmatrix} X' \ Y' \ Z' \end{bmatrix} = R \times \begin{bmatrix} X \ Y \ Z \end{bmatrix} + T $$

Ajouter une colonne de 1 pour obtenir les coordonnées homogènes :

$$ \begin{bmatrix} X' \ Y' \ Z' \ 1 \end{bmatrix} $$

Effectuer la multiplication matricielle avec la matrice intrinsèque de la caméra :

$$ \begin{bmatrix} u \ v \end{bmatrix} = \text{CameraMatrix} \times \begin{bmatrix} X' \\ Y' \\ Z' \\ 1 \end{bmatrix} $$

Normaliser pour obtenir les coordonnées en pixels u et v en divisant par la composante $Z'$ :

$$ \begin{bmatrix} u' \\ v' \end{bmatrix} = \begin{bmatrix} u \\ v \end{bmatrix} / Z' $$

Vidéos de présentation

• Lien playlist Youtube
• Lien pitch Youtube
• Lien tuto Youtube

Technologies et outils

• ROS2 : Intégration et communication
• Python : API et algorithmes
• CUDA : Opérations sur GPU
• OpenCV : Traitement d'image et visualisation côté utilisateur
• YOLOv8 : Reconnaissance et segmentation d'objets, personnes
• Deepface : Reconnaissance visages et attributs
• Rviz : visualisation des topics côté ROS2

Pré-requis

Environnement de dev :

• Ubuntu 22.04
• Nvidia RTX 2070 Super
• ROS2 Humble
• Python 3.10
• numpy 1.26.2
• ultralytics 8.0.227
• opencv-python 4.8.0.74
• CUDA 12.1

Installation

1. Installer ROS2 Humble

2. Installer les librairies

pip install -r requirements.txt

3. Installer CUDA et connaitre la version de CUDA (plusieurs façons)

nvcc --version
cat /usr/local/cuda/version.json
ls -l /usr/local | grep cuda # cuda supposément installé dans /usr/local/cuda

4. Installer la version de CuPy (NumPy sur GPU) selon la version de CUDA (ne pas remplacer x dans la version de CuPy)

# Pour CUDA 10.2
pip install cupy-cuda102

# Pour CUDA 11.0
pip install cupy-cuda110

# Pour CUDA 11.1
pip install cupy-cuda111

# Pour CUDA 11.2 ~ 11.x
pip install cupy-cuda11x

# Pour CUDA 12.x
pip install cupy-cuda12x

5. Installer dlib compilé avec CUDA

• Vérifier la version de nvcc

nvcc --version

• Installer cuDNN

• Installer dlib

pip uninstall dlib
# cloner le repo (dans /home par exemple)
git clone https://github.com/davisking/dlib.git
cd dlib
mkdir build
cd build


cmake .. -DDLIB_USE_CUDA=1 -DUSE_AVX_INSTRUCTIONS=1
cmake --build .

cd ..
python3 setup.py install

Si problème de compatibilité entre le compilateur (gcc/g++) et nvcc (le compilateur CUDA), chercher le compilateur compatible:
Tableau compatibilité nvcc et gcc

Installer la version spécifique de gcc et g++ (ex pour ver 9):

sudo apt update
sudo apt install gcc-9 g++-9

Préciser la version lors du build

pip uninstall dlib
cd dlib
mkdir build
cd build

CC=gcc-9 CXX=g++-9 cmake .. -DDLIB_USE_CUDA=1 -DUSE_AVX_INSTRUCTIONS=1
cmake --build .

cd ..
CC=gcc-9 CXX=g++-9 python3 setup.py install

6. Installer le SDK d'Intel® RealSense™

sudo apt install ros-humble-librealsense2*

7. Création du workspace ROS2 Humble

mkdir -p ~/ros2_humble_ws/src
cd ~/ros2_humble_ws/src/

8. Installer le wrapper Intel® RealSense™ ROS2 Clone dans src:

git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development
cd ~/ros2_humble_ws

9. Installer les dépendences

sudo apt-get install python3-rosdep -y
sudo rosdep init
rosdep update
rosdep install -i --from-path src --rosdistro humble --skip-keys=librealsense2 -y

Normalement si toutes les dépendances sont installées le terminal affiche:

#All required rosdeps installed successfully

Toujours dans le workspace humble, build et source:

colcon build
source install/setup.bash

9. (bis) Si le build ne passe pas (selon mon historique de commandes):

source install/setup.bash
colcon build
source install/setup.bash 
colcon build
rosdep install -i --from-path src --rosdistro humble -y
sudo apt-get update
rosdep install -i --from-path src --rosdistro humble -y
colcon build
source /opt/ros/humble/setup.bash 
source install/setup.bash 
colcon build
source /opt/ros/humble/setup.bash 
source install/setup.bash

10. Placer le package ROS2/robovision_ros dans ros2_humble_ws/src/ Build et source

colcon build
source install/setup.bash

11. Lancer le noeud de la camera dans un terminal

ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true align_depth.enable:=true pointcloud.ordered_pc:=true

12. Lancer les noeuds du package dans un autre terminal

ros2 launch robovision_ros launch.py

13. Lancer Rviz dans un autre terminal

rviz2