stereo.tcl

package require image
package require model3d


namespace eval stereo {
    #######################################################################
    #
    #   Stro pour les images stereo quelconques
    #
    #   Pour les noms des images, on essaie plusieurs regles
    #     (sur l'image droite seulement)  partir d'enttes $image_name:
    #
    #     ${image_name}${separateur}d${extension}
    #
    #     avec separateur = '.' ou '-' ou '_'
    #      et  extension  = ' ' ou '.tif' ou '.ras' avec .gz en plus ou sans
    #
    #
    #######################################################################
    
    
    proc stereo_correlation { 
      {image_dir "/home/ayman/images/dala/indoor/serie05/"}
      {image_name "image.037"}
      {type_rectification 1}
      {filtrage 0}
      {type_correlation 2}
      {type_interpolation 1}
      {window_size 11}
      {i_min 0.0}
      {i_max 1.0}
      {j_min 0.0}
      {j_max 1.0}
      {do_display true}
      {delcorrelimages true}
      {debayer false}
      } {
  # stereo_correlation: Macro qui lit deux images stro quelconques
  #                     et retourne une image de points 3D. 
  #
  # Paramtres :
  #
  #        image_dir: le nom du repertoire dans lequel sont les images.
  #                   Quelques infos sur:
  #                   il DOIT y avoir un lien ou un sous-rpertoire 
  #                   'vecteurs' dans lequel se trouvent les matrices
  #                   de calibrage
  #
  #        image_name: l'entte du nom des fichiers image, sans
  #                   l'extension
  #                   (voir commentaires ci dessus)
  #
  #
  #
  # Tous les paramtres qui suivent sont optionnels, il est conseill de
  # prendre leur valeur par dfaut ( moins de bien connatre les algos)
  #
  #
  #        type_rectification: 0 = pas de rectification
  #                            1 = rectification  partir des matrices
  #                            2 = rectification  partir des tables
  #                            3 = rectification "inverse"  
  #
  #        filtrage:   0 = pas de filtrage des images sources
  #                    1 = filtrage laplacien
  #                    2 = filtrage gaussien (pas implement...)
  #
  #        type_correlation: 0 = ZNCC
  #                          1 = ZNCC_Sigma
  #                          2 = Census
  #                          3 = Census_sigma 
  #
  #        i_min, i_max, j_min, j_max : prcise la zone des images
  #                             que l'on veut corrler
  #                             (en pourcentage compris entre 0 et 1) 
  #                             i pour les lignes et j pour les colonnes.
  #



  # suivent les diffrents paramtres sur le choix des algorithmes,
  # que l'on peut modifier si on veut ( tous ces paramtres 
  # correspondent des enumrations dfinies dans 
  # "enum_StereoCorrelation.h") pour les pr-traitements
  
  
  # Mise de cot des paths existants
  # set old_path_images $memory::path_images
  # set memory::path_images $image_dir
  set oldPath [pwd]
  cd $image_dir
  
  
  # Lecture des fichiers de calibrage
  set param_stereo_file "$image_dir/param.stereo"  
  set calibrageMatrices  [::stereo::new_CalibrageMatrices]
  ::stereo::CalibrageMatrices_readFromFile $calibrageMatrices \
      $param_stereo_file
  
  ######################################################################
  # Initialisations : lecture des images source et 
  # initialisatstereo_procions des toutes les structures
  # ncesssaires  la corrlation
  if { [file exists $image_dir/$image_name.d] } then {
        set separateur "."
        set extension ""
  } elseif { [file exists $image_dir/$image_name.d.gz] } then {
        set separateur "."
        set extension ".gz"
  } elseif { [file exists $image_dir/$image_name.d.Z] } then {
        set separateur "."
        set extension ""
  } elseif { [file exists $image_dir/$image_name.d.tif] } then {
        set separateur "."
        set extension ".tif"
  } elseif { [file exists $image_dir/$image_name.d.tif.gz] } then {
        set separateur "."
        set extension ".tif.gz"
  } elseif { [file exists $image_dir/$image_name.d.tiff] } then {
        set separateur "."
        set extension ".tiff"
  } elseif { [file exists $image_dir/$image_name.d.tiff.gz] } then {
        set separateur "."
        set extension ".tiff.gz"
  } elseif { [file exists $image_dir/$image_name.d.ras] } then {
        set separateur "."
        set extension ".ras"
  } elseif { [file exists $image_dir/$image_name.d.ras.gz] } then {
        set separateur "."
        set extension ".ras.gz"
  } elseif { [file exists $image_dir/$image_name-d] } then {
        set separateur "-"
        set extension ""
  } elseif { [file exists $image_dir/$image_name-d.gz] } then {
        set separateur "-"
        set extension ".gz"
  } elseif { [file exists $image_dir/$image_name.d.Z] } then {
        set separateur "."
        set extension ".Z"
  } elseif { [file exists $image_dir/$image_name.d.jpeg] } then {
        set separateur "."
        set extension ".jpeg"
  } elseif { [file exists $image_dir/$image_name-d.tif] } then {
        set separateur "-"
        set extension ".tif"
  } elseif { [file exists $image_dir/$image_name-d.tif.gz] } then {
        set separateur "-"
        set extension ".tif.gz"
  } elseif { [file exists $image_dir/$image_name-d.ras] } then {
        set separateur "-"
        set extension ".ras"
  } elseif { [file exists $image_dir/$image_name-d.ras.gz] } then {
        set separateur "-"
        set extension ".ras.gz"
  } elseif { [file exists $image_dir/${image_name}_d] } then {
        set separateur "_"
        set extension ""
  } elseif { [file exists $image_dir/${image_name}_d.gz] } then {
        set separateur "_"
        set extension ".gz"
  } elseif { [file exists $image_dir/$image_name.d.Z] } then {
        set separateur "."
        set extension ".Z"
  } elseif { [file exists $image_dir/${image_name}_d.tif] } then {
        set separateur "_"
        set extension ".tif"
  } elseif { [file exists $image_dir/${image_name}_d.tif.gz] } then {
        set separateur "_"
        set extension ".tif.gz"
  } elseif { [file exists $image_dir/${image_name}_d.ras] } then {
        set separateur "_"
        set extension ".ras"
  } elseif { [file exists $image_dir/${image_name}_d.ras.gz] } then {
        set separateur "_"
        set extension ".ras.gz"
  } else {
        puts "Fichiers image non trouvs!!"
        return
  }

  puts "loading of image : ${image_dir}${image_name}${separateur}g$extension "
  set source_image_gauche [image::Image_loadImage \
        ${image_dir}${image_name}${separateur}g$extension]
  puts "loading of image : ${image_dir}${image_name}${separateur}d$extension "
  set source_image_droite [image::Image_loadImage \
        ${image_dir}${image_name}${separateur}d$extension]
  if { $debayer } {
      set interm [image::new_Image [$source_image_gauche width] \
          [$source_image_gauche height] \
          [$source_image_gauche img_depth] \
          $::image::JfrImage_CS_BGR]
      image::Image_convertColorMode $source_image_gauche $interm \
    $::image::Image_bayerRG2bgr
      image::Image_convertColorMode $interm $source_image_gauche \
    $::image::Image_rgb2grey
      image::Image_convertColorMode $source_image_droite $interm \
    $::image::Image_bayerRG2bgr
      image::Image_convertColorMode $interm $source_image_droite \
    $::image::Image_rgb2grey
      image::delete_Image $interm
  }
  
  set im3d [_stereo_correlation $source_image_gauche \
    $source_image_droite $calibrageMatrices \
    $type_rectification $filtrage $type_correlation \
    $type_interpolation $window_size \
    $i_min $i_max $j_min $j_max $do_display $delcorrelimages ]

  ::stereo::delete_CalibrageMatrices $calibrageMatrices
  ::image::delete_Image $source_image_gauche
  ::image::delete_Image $source_image_droite

  # Restauration des chemins d'accs aux donnes image et vecteurs 
  cd $oldPath
  
  ::model3d::Image3D_minMax $im3d
  ::model3d::Image3D_info $im3d

  return $im3d
}
    
    
    proc _stereo_correlation { 
      { source_image_gauche }
      { source_image_droite }
      { calibrageMatrices }
      {type_rectification 1}
      {filtrage 0}
      {type_correlation 0}
      {type_interpolation 1}
      {window_size 11}
      {i_min 0.0}
      {i_max 1.0}
      {j_min 0.0}
      {j_max 1.0}
      {do_display true}
      {delcorrelimages true}
  } {

  set LEFT_IMAGE_ID  0
  set RIGHT_IMAGE_ID 1
  
  # do_reduction flag
  #       0 : pas de reduction
  #       1 : avec reduction
  set do_reduction 1;
  
  # typre_reduction: type de reduction:
  #        0 = sous-echantillonage
  #        1 = rduction-moyennage
  set type_reduction 0; 
  
  # Do_Interpolation flag pendant la rectification : 
  #       0 : non
  #       1 : oui
  set do_interpolation 1;
  
  # Correction de la distorsion pendant la rectification :
  #           0: non
  #           1: nouveau modle en R
  #           2: modle en R3
  # see the enum:
  #      enum DISTORTION_TYPE{ DISTORTION_NIL,
  #                DISTORTION_R,
  #                      DISTORTION_R3};
  set distorsion_type 2;
  
  # Les tailles des images originales
  
  set disp_min -100;
  set disp_max 0;
  
  # voir enum
  # enum CENSUS_TYPE {CENSUS_NIL, 
  #             CENSUS_8BITS_SQUARE,
  #                   CENSUS_8BITS_DIAMOND,
  #                   CENSUS_MEAN_8BITS_SQUARE,
  #                   CENSUS_MEAN_8BITS_DIAMOND,
  #                   CENSUS_MEAN_MEAN_8BITS_SQUARE,
  #                   CENSUS_MEAN_MEAN_8BITS_DIAMOND};
  set type_census 4;
  
  # Pour le filtrage final et la reconstruction
  #        0 : pas de blob filtre de l'image de disparits
  #        1 : avec   blob filtre de l'image de disparits
  set do_blob_filter 1;   

  set th_filter_disparity1 500
  set th_filter_disparity2 1.0

  # tpye_repere :   
  #         0 : reconstruction dans le repre rectifi
  #         1 : reconstruction dans le repre original
  set type_repere 0;

  
  # Les paramtres pour le filtrage laplacien avant corrlation
  if {$filtrage == 1} {
      set size_low 3;
      set size_high 7;
      set coefficient 2;
  }
  
# set th_sigma  1.0;  # Seuil sur l'cart type des pixels
# set th_score  0.0;  # Seuil sur la valeur du score max
# set th_delta  0.0;  # Seuil sur (score max - 2nd score)
# set th_voisins  0.0;  # Seuil sur la forme du pic de corrlation
# set espilon_inverse 1;  # Seuil sur la diff entre corrlation
                          #   directe et inverse
  set th_sigma  1.0;  # Seuil sur l'cart type des pixels
  set th_score  0.5;  # Seuil sur la valeur du score max
  set th_delta  0.05; # Seuil sur (score max - 2nd score)
  set th_voisins  0.0;  # Seuil sur la forme du pic de corrlation
  set espilon_inverse 1;  # Seuil sur la diff entre corrlation
                          #   directe et inverse
  
  
  # Tout les variables qui suivent sont en global juste pour le debug
  # (en fait on les libre  la fin)
  global im_reduite_gauche 
  #     globalsource_gauche im_rectifiee_gauche 
  #     global im_reduite_droite source_droite im_rectifiee_droite
  global im_reduite_droite
  #     global images_sizes
  #     global correlation_params
  #     global rectif_params_gauche rectif_params_droite
  #     global images_tmp_g images_tmp_d
        global correlation_results

  

  # affichage de l'image gauche
  if { $do_display } {
      ::display::show $source_image_gauche "image source gauche"
      #::display::show $source_image_droite "image source droite"
  }


  # nblig :  nombre de lignes dans l'image (hieght)
  # nbcol :  nombre de colonnes dans l'image (width)
  set nbcol [::image::CvImage_width $source_image_gauche]
  set nblig [::image::CvImage_height $source_image_gauche]
  

  # Les tailles des images
  set images_sizes [::stereo::new_ImagesSizes]
  ::stereo::ImagesSizes_initialize $images_sizes    \
      $nblig $nbcol 1 1  \
      $i_min $i_max $j_min $j_max

  puts "Info ImagesSizes before reinitialize"
        ::stereo::ImagesSizes_info $images_sizes
  puts "============================"

  

  # re-initialize ImagesSizes object to get the new data
  ::stereo::ImagesSizes_initialize $images_sizes $calibrageMatrices

  puts "Info ImagesSizes after reinitialize"
        ::stereo::ImagesSizes_info $images_sizes
  puts "============================"
  puts "============================"
  puts "============================"
  
  
  # Les paramtres de corrlation
  set correlation_params [::stereo::new_CorrelationParams]
  ::stereo::CorrelationParams_initialize $correlation_params \
      $type_correlation  $type_interpolation  \
      $window_size $window_size $disp_min  $disp_max  \
      $th_sigma  $th_score  $th_delta  $th_voisins \
      $espilon_inverse
  
  ::stereo::CorrelationParams_info $correlation_params
  
  # La structure qui contient les rsultats de la corrlation
  set correlation_results [::stereo::new_CorrelationResults]
  ::stereo::CorrelationResults_initialize $correlation_results \
      $images_sizes  $correlation_params
  
  ::stereo::CorrelationResults_info $correlation_results
  
  
  # Les images intermdiaires
  set images_tmp_g [::stereo::new_ImagesTemporary]
  ::stereo::ImagesTemporary_initialize $images_tmp_g $images_sizes
  
  set images_tmp_d [::stereo::new_ImagesTemporary]
  ::stereo::ImagesTemporary_initialize $images_tmp_d $images_sizes
  
  set rectif_params_droite [::stereo::new_RectificationParams]
  ::stereo::RectificationParams_initialize $rectif_params_droite \
      $images_sizes $calibrageMatrices $RIGHT_IMAGE_ID
  

  set rectif_params_gauche [::stereo::new_RectificationParams]
  ::stereo::RectificationParams_initialize $rectif_params_gauche \
      $images_sizes $calibrageMatrices $LEFT_IMAGE_ID

  
  # On initialise les tables s'il le faut
  if {$type_rectification == 2} {
      ::stereo::RectificationParams_initializeTables_michel \
    $rectif_params_gauche $distorsion_type
      ::stereo::RectificationParams_initializeTables_michel \
    $rectif_params_droite $distorsion_type
  }
  
  
  # Reconstruction Params
  set reconstruction_params [::stereo::new_ReconstructionParams]
  ::stereo::ReconstructionParams_initialize \
      $reconstruction_params $calibrageMatrices

  

  ######################################################################
  # Rectification 
  # first we must create the rectified image buffer 
  # then call the rectification function
  set nRectifiedImageWidth \
      [::stereo::ImagesSizes_getRectifiedImageWidth $images_sizes]
  set nRectifiedImageHeight \
      [::stereo::ImagesSizes_getRectifiedImageHeight $images_sizes]

  set img_depth $::image::IPL_DEPTH_8U
  set color_space  $::image::JfrImage_CS_GRAY
  
  set im_reduite_gauche [::image::new_Image \
           $nRectifiedImageWidth \
           $nRectifiedImageHeight\
           $img_depth \
           $color_space]
  
  
  set im_reduite_droite [::image::new_Image \
           $nRectifiedImageWidth \
           $nRectifiedImageHeight\
           $img_depth \
           $color_space]
  
  
  ::stereo::jfrRectification $source_image_gauche $im_reduite_gauche \
      $images_sizes $rectif_params_gauche \
      $do_reduction $type_rectification\
      $distorsion_type $do_interpolation
  
  
  ::stereo::jfrRectification $source_image_droite $im_reduite_droite \
      $images_sizes $rectif_params_droite \
      $do_reduction $type_rectification\
      $distorsion_type $do_interpolation

  # Display the resulting images
  if { $do_display } {
      ::display::show $im_reduite_gauche "image reduite gauche"
      ::display::show $im_reduite_droite "image reduite droite"
  }

  
  ######################################################################
  # Filtrage
  if {$filtrage == 1} {
      ::stereo::laplacien_ima1 $im_reduite_gauche $images_tmp_g \
    $size_low $size_low $size_high $size_high $coefficient
      ::stereo::laplacien_ima1 $im_reduite_droite $images_tmp_d \
    $size_low $size_low $size_high $size_high $coefficient
  }


  
  ######################################################################
  # Pre-traitements avant correlation
  if { [expr {$type_correlation == 0} || {$type_correlation == 1}] } {
      # ZNCC: calcul des moyennes et sigmas
      ::stereo::jfrMoyennesSigmas $im_reduite_gauche \
    $images_tmp_g $correlation_params
      ::stereo::jfrMoyennesSigmas $im_reduite_droite \
    $images_tmp_d $correlation_params
      
  } else { #  Census
      if {[expr {$type_census == 3} || \
         {$type_census == 4} || \
         {$type_census == 5} || \
         {$type_census == 6}]} {
    # Il faut calculer l'images des moyennes
    ::stereo::jfrMoyennes $im_reduite_gauche 3 3 $images_tmp_g
    ::stereo::jfrMoyennes $im_reduite_droite 3 3 $images_tmp_d
      }
      
      # Creation des images de Census
      set im_census_gauche [::image::Image im_census_gauche \
              $nRectifiedImageWidth \
              $nRectifiedImageHeight\
              $img_depth \
              $color_space]
      
      set im_census_droite [::image::Image im_census_droite \
              $nRectifiedImageWidth \
              $nRectifiedImageHeight\
              $img_depth \
              $color_space]
      
      ::stereo::jfrTransform_census_8bits  \
    $im_reduite_gauche $images_tmp_g \
    $type_census $im_census_gauche
      
      ::stereo::jfrTransform_census_8bits  \
    $im_reduite_droite $images_tmp_d \
    $type_census  $im_census_droite
  }
  
  
  
  
  ######################################################################
  # Corrlation
  
  if {[expr {$type_correlation == 0} || {$type_correlation == 1}]} {
      # ZNCC
      ::stereo::jfrCorrelationZncc \
    $im_reduite_gauche $im_reduite_droite \
    $images_tmp_g $images_tmp_d \
    $correlation_params $correlation_results
  } else {
      # Census
      ::stereo::jfrCorrelationCensus \
    $im_census_gauche $im_census_droite \
    $images_sizes $correlation_params $correlation_results
  }
  
  
  # Affichage des disparits non filtres
  set im_disparity \
      [::stereo::CorrelationResults_getImageForDisparity \
     $correlation_results]
  
  if { $do_display } {
      # ::display::show $im_disparity "image of not filtered disparity "
  }

  ## todo

  ######################################################################
  # Filtrage
  if {$do_blob_filter != 0} {
      ::stereo::jfrBlobFilterDisparites $correlation_results \
    $th_filter_disparity1 $th_filter_disparity2
  }
  
  ####  "Disparites filtrees"
  # Affichage des disparits filtres
  set im_disparity_fltr \
      [::stereo::CorrelationResults_getImageForDisparity \
     $correlation_results]
  
  if { $do_display } {
      ::display::show $im_disparity_fltr "image of filtered disparity "
  }
  
  # Affichage ventuel des pixels corrls
  set im_correlated_pixels \
      [::stereo::CorrelationResults_getImageOfCorrelatedPixels \
     $correlation_results $im_reduite_gauche]
  if { $do_display } {
      ::display::show $im_correlated_pixels "image of correlated pixels"
  }
  
  # display of regions
  set im_regions \
      [::stereo::CorrelationResults_getImageForRegions \
     $correlation_results]
  if { $do_display } {
      # ::display::show $im_regions "image of regions"
  }
  
  ######################################################################
  # Reconstruction.
  # On dtermine si on reconstruit les variances ou non

  if {[expr {$type_correlation == 1} || {$type_correlation == 3}]} {
      set type_reconstruction 1
  } else { set type_reconstruction 0 }
  

  # first creat image3D
  set im3d [::model3d::Image3D]
  ::model3d::Image3D_initialize $im3d \
      $nRectifiedImageWidth $nRectifiedImageHeight


  ::stereo::jfrReconstruction3D $correlation_results \
      $images_sizes $type_repere \
      $type_reconstruction $reconstruction_params \
      $rectif_params_gauche $im3d
  
  
  ::model3d::Image3D_minMax $im3d
  

  # clean intermediaire variables
  ::stereo::delete_ImagesSizes $images_sizes
  # ::stereo::delete_CalibrageMatrices $calibrageMatrices
  ::stereo::delete_CorrelationParams $correlation_params
  # ::stereo::delete_CorrelationResults $correlation_results
  ::stereo::delete_ImagesTemporary $images_tmp_g
  ::stereo::delete_ImagesTemporary $images_tmp_d
  ::stereo::delete_RectificationParams $rectif_params_droite
  ::stereo::delete_RectificationParams $rectif_params_gauche
  ::stereo::delete_ReconstructionParams $reconstruction_params
  if { $delcorrelimages} {
      ::image::delete_Image $im_reduite_gauche
      ::image::delete_Image $im_reduite_droite
  }

  ::image::delete_Image $im_correlated_pixels
  ::image::delete_Image $im_disparity

  return $im3d
    }
    
}

package provide stereo 0.1