synchro_camera

Table des matières

Programme sur Arduino
Noeud pour l'enregistrement des images au format PPM
Résultats
BagFromFiles node
Modifications pour le cas où les images stéréos sont inclinées à 90 degrés

Programme sur Arduino

voir Diviseur de fréquence + affichage nombre de fronts descendants sur MAX7221 sur timer: https://bvdp.inetdoc.net/wiki/doku.php?do=export_code&id=timer&codeblock=0

Noeud pour l'enregistrement des images au format PPM

La camera de gauche est master, elle envoie un signal à la caméra de droite et à elle même pour démarrer la prise de photo. L'arduino récupère ce signal pour incrémenter un compteur qu'il affiche sur l'afficheur 8 segments.

Création d'un catkin etc

Pour créer un catkin initialement

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

il faut charger les variables d'environnement pour ce catkin (normalement on stocke le source … dans ~/.bashrc)

cd ~/catkin_ws/
source devel/setup.bash
echo $ROS_PACKAGE_PATH

Pour péréniser:

echo "source ~/catkin_ws/devel/setup.bash" >>~/.bashrc

Récupération du node ueye_cam

Installation noeud ueye modifié par Ariel et Jessica:

cd ~/catkin_ws/src
#changé de git clone ssh://git@redmine.laas.fr/laas/users/apodlubn/ueye_cam.git en:
git clone https://github.com/bvandepo/ueye_cam.git

pour visualiser l'image en provenance d'une caméra:

cd ~/catkin_ws
roslaunch ueye_cam rgb8.launch 
rosrun image_view image_view image:=/camera/image_raw
rostopic list
rostopic echo /camera/camera_info

pour étalonner en monoculaire avec une seule petite mire:

rosrun camera_calibration cameracalibrator.py --size 7x5 --square 0.01 image:=/camera/image_raw

pour étalonner en monoculaire avec deux mires:

rosrun camera_calibration cameracalibrator.py --size 7x6 --square 0.108 --size 8x5 --square 0.029 image:=/camera/image_raw

Création du node ueye_cam_test pour test synchro

pour créer un nouveau node vide avec les dépendances adéquates

cd ~/catkin_ws/src
catkin_create_pkg ueye_cam_test camera_calibration_parsers   camera_info_manager   image_transport   nodelet   roscpp   sensor_msgs   std_msgs   cv_bridge

compléter le fichier créé en ajoutant à la fin de src/ueye_cam_test/CMakeLists.txt:

CMakeLists.txt

###############BVANDEPO##############
  include_directories(include ${catkin_INCLUDE_DIRS})
  add_executable(ueye_cam_test_exe src/ueye_cam_test.cpp)
  target_link_libraries(ueye_cam_test_exe ${catkin_LIBRARIES})

copier le fichier cpp source dans:

~/catkin_ws_/src/ueye_cam_test/src/ueye_cam_test.cpp

ueye_cam_test.cpp

// from ueyecam_nodelet
#include <ros/package.h>
#include <camera_calibration_parsers/parse.h>
#include <sensor_msgs/fill_image.h>
#include <sensor_msgs/image_encodings.h>
 
#include <sstream>
#include "std_msgs/String.h"
 
// code sample image transport
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
 
// For time synchronizer
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
 
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <cstring>
 
using namespace sensor_msgs;
using namespace message_filters;
 
unsigned int k(0);
 
/*****************************************************/
/***                  SAVE_PPM                     ***/
/*****************************************************/
void save_ppm(char *filename, char *im, unsigned int width, unsigned int height)
{
    FILE *f;
    if(!(f=fopen(filename,"wb"))) {
        fprintf(stderr,"Could not open `%s'",filename);
        exit(1);
    }
 
    fprintf(f,"P6\n%d %d\n255\n",width,height);
    fwrite(im,1,width*height*3,f);
    fclose(f);
}
/*****************************************************/
/***               SAVE_PPM_NAMED                  ***/
/*****************************************************/
void save_ppm_named(char *basename, int k,char *endname,cv_bridge::CvImagePtr  cv_ptr_1)
{
    std::stringstream sstm;
    std::string nameFileL;
    sstm << basename << std::setfill('0') << std::setw(5) << k << endname;
    nameFileL = sstm.str();
    save_ppm((char*)nameFileL.c_str(), (char*)cv_ptr_1->image.data, cv_ptr_1->image.cols, cv_ptr_1->image.rows);
}
/*****************************************************/
/***              IMAGE CALLBACK                   ***/
/*****************************************************/
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(msg, "bgr8");	
	save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_mono.ppm",cv_ptr_1);
        cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
        cv::waitKey(30);
        k++;
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}
/*****************************************************/
/***              STEREO CALLBACK                  ***/
/*****************************************************/
void stereoCallback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
    // Solve all of perception here...
    try
    {
        cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(image1, "bgr8");
        cv_bridge::CvImagePtr cv_ptr_2 = cv_bridge::toCvCopy(image2, "bgr8");
        // save left image
	save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_left.ppm",cv_ptr_1);
        // save right image
	save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_right.ppm",cv_ptr_2);
        // show image
        cv::imshow("view_left", cv_ptr_1->image);
        cv::imshow("view_right", cv_ptr_2->image);
        cv::waitKey(30);
        k++;
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", image1->encoding.c_str());
    }
}
/*****************************************************/
/***                    MAIN                       ***/
/*****************************************************/
int main(int argc, char **argv)
{
    // create folder where we save images
    system("mkdir /tmp/images");
 
    ros::init(argc, argv, "listenerUeye");
    ros::NodeHandle nh;
    bool stereo(true);
 
    if(!stereo){
	ROS_INFO("ueye_cam_test in moncular mode"); 
        cv::namedWindow("view");
        cv::startWindowThread();
 
        image_transport::ImageTransport it(nh);
        image_transport::Subscriber sub = it.subscribe("/camera/image_raw", 1, imageCallback);
 
        ros::spin();
 
        cv::destroyWindow("view");
    }
    else{
	ROS_INFO("ueye_cam_test in binocular mode");
        cv::namedWindow("view_left");
        cv::namedWindow("view_right");
        cv::startWindowThread();
 
        message_filters::Subscriber<Image> image_left_sub(nh, "/stereo/left/image_raw", 1);
        message_filters::Subscriber<Image> image_right_sub(nh, "/stereo/right/image_raw", 1);
        TimeSynchronizer<Image, Image> sync(image_left_sub, image_right_sub, 10);
        sync.registerCallback(boost::bind(&stereoCallback, _1, _2));
 
        ros::spin();
 
        cv::destroyWindow("view_left");
        cv::destroyWindow("view_right");
    }
}

Adapter dans ~/catkin_ws_/src/ueye_cam_test/src/ueye_cam_test.cpp la ligne pour s'abonner au topic image/raw ou /stereo/right/image_raw et /stereo/left/image_raw

bool stereo(false);  ou  bool stereo(true);

pour compiler:

cd ~/catkin_ws/
catkin_make

Il n'y a pas de launch. Pour lancer le programme en monoculaire:

roscore
roslaunch ueye_cam rgb8.launch 
rosrun image_view image_view image:=/camera/image_raw
rosrun ueye_cam_test ueye_cam_test_exe

TODO: expliquer pour lancer en stéréo et tester

Résultats

BagFromFiles node

Node créé par Raul Mur Artal et adapté par Ariel pour gérer les séquences en stéréo

Nous l'utilisons pour regénérer un fichier bag contenant une séquence d'images précédemment enregistrée par l'outils cameracalibrator.py dans le fichier calibrationdata.tar.gz . Ceci sert à pouvoir réutiliser une séquence en simulant une (paire de) caméra(s) à partir d'un fichier en utilisant rosplay.

il y a déjà pas mal d'explications sur: https://bvdp.inetdoc.net/wiki/doku.php?id=ueye#camera_synchronization

Pour capturer les images depuis les 2 caméras en stéréo:

cd ~/catkin_ws
roslaunch ueye_cam stereo.launch

Pour lancer l'étalonnage et sauver automatiquement les images en /tmp/calibrationdata.tar.gz

rosrun camera_calibration cameracalibrator.py --size 7x6 --square 0.108 --size 8x5 --square 0.029 right:=/stereo/right/image_raw left:=/stereo/left/image_raw
click calibrate
click save

cd ~/catkin_ws
cd src
git clone ssh://git@redmine.laas.fr/laas/users/danes/apodlubn/bagfromimages.git
echo "export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/home/bvandepo/catkin_ws/src/bagfromimages" >>~/.bashrc
mkdir build
cd build
cmake ..
make
sudo make install
cd ..

mkdir -p ~/2ears/seq1
cd ~/2ears/seq1

y écrire ce fichier:

constructbag.sh

tar xvf calibrationdata.tar.gz 
mkdir left
mkdir right
mv left-* left/
mv right-* right/
FOLDER=`pwd`
echo $FOLDER
rosrun bagfromimages bagfromimages  $FOLDER/left/ $FOLDER/right/ .png 2 $FOLDER/images.bag
rm -rf left/
rm -rf right/

chmod a+x constructbag.sh
./constructbag.sh

Le script génère dans le dossier courant un fichier images.bag

Les paramètres passés à rosrun bagfromimages bagfromimages sont les suivants:

1: Left images folder
2: Right images folder
3: image format
4: number of frames per second to export to the bag file
5: bag's output

Pour rejouer une séquence enregistrée:

cd ~/2ears/seq1
rosbag play images.bag

Modifications pour le cas où les images stéréos sont inclinées à 90 degrés

if the rectified images appears small in a corner, apply this change to the .py:

gedit  /opt/ros/kinetic/lib/python2.7/dist-packages/camera_calibration/calibrator.py

Après:

         cv2.stereoCalibrate(opts, lipts, ripts,
                             self.l.intrinsics, self.l.distortion,
                             self.r.intrinsics, self.r.distortion,
                             self.size,
                             self.R,                            # R
                             self.T,                            # T
                             criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
                             flags = flags)

Changer:

self.set_alpha(0)

en:

self.set_alpha(-1.0)

pour calculer la différence entre les 2 fichier et générer le patch:

cd ~/2ears
diff calibrator.py /opt/ros/kinetic/lib/python2.7/dist-packages/camera_calibration/calibrator.py  >>calibrator.diff
sudo patch /opt/ros/kinetic/lib/python2.7/dist-packages/camera_calibration/calibrator.py calibrator.diff

create the ini files from yaml files:

rosrun camera_calibration_parsers convert left.yaml left.ini
rosrun camera_calibration_parsers convert right.yaml right.ini

Script pour convertir les résultats d'étalonnage en yaml et lancer le noeuds d'acquisition des images en stéréo avec les caméras réelles:

debugscript.sh

#!/bin/bash
rosrun camera_calibration_parsers convert /home/bvandepo/.ros/camera_conf/stereo/left.ini /home/bvandepo/.ros/camera_info/stereo/left.yaml
rosrun camera_calibration_parsers convert /home/bvandepo/.ros/camera_conf/stereo/right.ini /home/bvandepo/.ros/camera_info/stereo/right.yaml
source ~/catkin_ws/devel/setup.bash
roslaunch ueye_cam stereo.launch

Script pour visualiser les images en provenance des caméras et les images épipolaires réctifiées:

image_procDebug.sh

#ROS_NAMESPACE=stereo rosrun stmage_proc stereo_image_proc &
rosrun image_view image_view image:=/stereo/left/image_raw &
rosrun image_view image_view image:=/stereo/left/image_rect_color &
rosrun image_view image_view image:=/stereo/right/image_raw &
rosrun image_view image_view image:=/stereo/right/image_rect_color &
read -p "Press any key"
pkill image_view 
#pkill stereo_image_proc
#pkill ueye_cam