Outils pour utilisateurs

Outils du site


synchro_camera

Différences

Ci-dessous, les différences entre deux révisions de la page.

Lien vers cette vue comparative

Les deux révisions précédentes Révision précédente
Prochaine révision
Révision précédente
synchro_camera [2016/11/04 14:07]
bvandepo
synchro_camera [2016/11/17 00:36] (Version actuelle)
bvandepo
Ligne 1: Ligne 1:
-{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00000left.png?600}}+=====Programme sur Arduino===== 
 +voir Diviseur de fréquence + affichage nombre de fronts descendants sur MAX7221 sur [[timer]]: http://homepages.laas.fr/bvandepo/wiki/doku.php?do=export_code&id=timer&codeblock=0  
  
 +=====Noeud pour l'enregistrement des images au format PPM=====
  
-{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00000right.png?600}}  
  
-{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00008left.png?600}}+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 photoL'arduino récupère ce signal pour incrémenter un compteur qu'il affiche sur l'afficheur 8 segments
  
-{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00008right.png?600}}+====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
  
  
-=====Noeud pour l'enregistrement des images au format PPM===== +====Récupération du node ueye_cam==== 
-<file cpp test_ueye.cpp>+ 
 +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:   
 +<file 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})  
 +</file> 
 + 
 +copier le fichier cpp source dans: 
 +  ~/catkin_ws_/src/ueye_cam_test/src/ueye_cam_test.cpp 
 + 
 +<file cpp ueye_cam_test.cpp>
 // from ueyecam_nodelet // from ueyecam_nodelet
 #include <ros/package.h> #include <ros/package.h>
Ligne 57: Ligne 107:
     fclose(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                   ***/ /***              IMAGE CALLBACK                   ***/
Ligne 65: Ligne 125:
     try     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::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
         cv::waitKey(30);         cv::waitKey(30);
 +        k++;
     }     }
     catch (cv_bridge::Exception& e)     catch (cv_bridge::Exception& e)
Ligne 73: Ligne 136:
     }     }
 } }
- 
 /*****************************************************/ /*****************************************************/
 /***              STEREO CALLBACK                  ***/ /***              STEREO CALLBACK                  ***/
Ligne 84: Ligne 146:
         cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(image1, "bgr8");         cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(image1, "bgr8");
         cv_bridge::CvImagePtr cv_ptr_2 = cv_bridge::toCvCopy(image2, "bgr8");         cv_bridge::CvImagePtr cv_ptr_2 = cv_bridge::toCvCopy(image2, "bgr8");
- 
         // save left image         // save left image
-        std::stringstream sstm; + save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_left.ppm",cv_ptr_1);
-        std::string nameFileL; +
-        sstm << "/tmp/images/ueye_" << std::setfill('0'<< std::setw(5) << k << "_left" << ".ppm" +
-        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); +
         // save right image         // save right image
-        std::string nameFileR; + save_ppm_named((char*)"/tmp/images/ueye_", k,(char*)"_right.ppm",cv_ptr_2);
-        sstm.str(std::string()); +
-        sstm << "/tmp/images/ueye_" << std::setfill('0'<< std::setw(5) << k << "_right" << ".ppm" +
-        nameFileR = sstm.str(); +
-        save_ppm((char*)nameFileR.c_str()(char*)cv_ptr_2->image.data, cv_ptr_2->image.cols, cv_ptr_2->image.rows); +
         // show image         // show image
         cv::imshow("view_left", cv_ptr_1->image);         cv::imshow("view_left", cv_ptr_1->image);
Ligne 110: Ligne 161:
     }     }
 } }
- 
 /*****************************************************/ /*****************************************************/
 /***                    MAIN                       ***/ /***                    MAIN                       ***/
Ligne 124: Ligne 174:
  
     if(!stereo){     if(!stereo){
 + ROS_INFO("ueye_cam_test in moncular mode"); 
         cv::namedWindow("view");         cv::namedWindow("view");
         cv::startWindowThread();         cv::startWindowThread();
  
         image_transport::ImageTransport it(nh);         image_transport::ImageTransport it(nh);
-        image_transport::Subscriber sub = it.subscribe("/stereo/left/image_raw", 1, imageCallback);+        image_transport::Subscriber sub = it.subscribe("/camera/image_raw", 1, imageCallback);
  
         ros::spin();         ros::spin();
Ligne 135: Ligne 186:
     }     }
     else{     else{
 + ROS_INFO("ueye_cam_test in binocular mode");
         cv::namedWindow("view_left");         cv::namedWindow("view_left");
         cv::namedWindow("view_right");         cv::namedWindow("view_right");
Ligne 150: Ligne 202:
     }     }
 } }
 +</file>
  
 +
 +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=====
 +{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00000left.png?600}}
 +
 +
 +{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00000right.png?600}} 
 +
 +{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00008left.png?600}}
 +
 +{{http://homepages.laas.fr/bvandepo/files/synchro_camera/_calibUeye-00008right.png?600}}
 +
 +
 +=====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: http://homepages.laas.fr/bvandepo/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:
 +<file sh 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/
 </file> </file>
 +  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:
 +  
 +<file sh 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 
 +</file>
 +
 +Script pour visualiser les images en provenance des caméras et les images épipolaires réctifiées:
 +<file sh 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
 +</file>
 +
synchro_camera.1478264824.txt.gz · Dernière modification: 2016/11/04 14:07 de bvandepo