vme.tcl

# $Id$ #

#
# /** VME macro. \file vme.tcl  \ingroup vme */
#

namespace eval vme {

    proc estimateMotion {indexFrame1 indexFrame2
       { scale 0.5 }
       { dataDir "." }
       { enableWeight "true" }
       { enableOutliers "true" }
       { enableUncertainty "true"}
       { minDisparity 0.5 }
       { maxStereoDeltaV 1.0 }
       { nbMin3DPointPairs 3 }
       { removeOutliersKmax 5 }
       { removeOutliersKmin 3 }
       { sigmaPix 0.5 }
       { sigmaDisp 0.5 }
       { doDisplay true }
       { imagesSubDir "images" }
       { configSubDir "config" }
         } {
  # estimate motion between two stereo frames
  # this macro uses module stereo to correct stereo images pairs

  # image reader
  set stereoImageReader [::image::new_StereoImageSequenceReader]
  $stereoImageReader setup [file join $dataDir $imagesSubDir] "image." ".g" ".d" "" 3

  # stereo bench model
  set sb [::image::new_StereoBench]
  $sb load [file join $dataDir $configSubDir "stereo.cal"]
  $sb applyScale $scale

  # vme params
  set vmeParams [vme::new_EngineParams \
         $enableWeight $enableOutliers $enableUncertainty \
         $minDisparity $maxStereoDeltaV \
         $nbMin3DPointPairs \
         $removeOutliersKmax $removeOutliersKmin \
         $sigmaPix $sigmaDisp]

  # vme engine
  global vmeEngine
  set vmeEngine [::vme::new_Engine $::geom::T3D_EULER $sb]
  $vmeEngine setParams $vmeParams

  ::image::delete_StereoBench $sb
  vme::delete_EngineParams $vmeParams

  # load images (1)
  set imageFrame1Left  [$stereoImageReader loadLeft  $indexFrame1]
  set imageFrame1Right [$stereoImageReader loadRight $indexFrame1]

  # rectif
  set imagesFrame1_rect [::stereo::rectification_old $imageFrame1Left $imageFrame1Right [file join $dataDir $configSubDir "vecteurs"] $scale]
  set imageFrame1Left_rect [lindex $imagesFrame1_rect 0]
  set imageFrame1Right_rect [lindex $imagesFrame1_rect 1]

  # debug
  $imageFrame1Left_rect saveImage "pipo1LeftRect.tiff"
  $imageFrame1Right_rect saveImage "pipo1RightRect.tiff"

  # free memory
  ::image::delete_JfrImage $imageFrame1Left
  ::image::delete_JfrImage $imageFrame1Right

  # init vme
  $vmeEngine init $imageFrame1Left_rect $imageFrame1Right_rect

  if $doDisplay {
      ::display::show $imageFrame1Left_rect "frame $indexFrame1 - left - corrected"
      ::display::show $imageFrame1Right_rect "frame $indexFrame1 - right - corrected"
      ::gfm::displayHarrisPoints $imageFrame1Left_rect [[$vmeEngine getStereoResult] cget -points_ref]
      ::gfm::displayHarrisPoints $imageFrame1Right_rect [[$vmeEngine getStereoResult] cget -points_match]
      ::gfm::displayMatch $imageFrame1Left_rect $imageFrame1Right_rect \
    [[$vmeEngine getStereoResult] cget -matches] \
    false "green"
  }

  # load images (2)
  set imageFrame2Left  [$stereoImageReader loadLeft  $indexFrame2]
  set imageFrame2Right [$stereoImageReader loadRight $indexFrame2]

  # rectif
  set imagesFrame2_rect [::stereo::rectification_old $imageFrame2Left $imageFrame2Right [file join $dataDir $configSubDir "vecteurs"] $scale]
  set imageFrame2Left_rect [lindex $imagesFrame2_rect 0]
  set imageFrame2Right_rect [lindex $imagesFrame2_rect 1]

  # debug
  $imageFrame2Left_rect saveImage "pipo2LeftRect.tiff"
  $imageFrame2Right_rect saveImage "pipo2RightRect.tiff"

  # free memory
  ::image::delete_JfrImage $imageFrame2Left
  ::image::delete_JfrImage $imageFrame2Right

  # vme
  $vmeEngine estimateMotion $imageFrame2Left_rect $imageFrame2Right_rect

  if $doDisplay {
      ::display::show $imageFrame2Left_rect "frame $indexFrame2 - left - corrected"
      ::display::show $imageFrame2Right_rect "frame $indexFrame2 - right - corrected"

      ::gfm::displayHarrisPoints $imageFrame2Left_rect [[$vmeEngine getStereoResult] cget -points_ref]
      ::gfm::displayHarrisPoints $imageFrame2Right_rect [[$vmeEngine getStereoResult] cget -points_match]
      ::gfm::displayMatch $imageFrame2Left_rect $imageFrame1Right_rect \
    [[$vmeEngine getStereoResult] cget -matches] \
    false "green"

      ::hpm::displayTrack $imageFrame2Left_rect \
    [[$vmeEngine getTrackResult] cget -matches]
  }

  # free memory
  ::image::delete_JfrImage $imageFrame1Left_rect
  ::image::delete_JfrImage $imageFrame1Right_rect
  ::image::delete_JfrImage $imageFrame2Left_rect
  ::image::delete_JfrImage $imageFrame2Right_rect

  ::image::delete_StereoImageSequenceReader $stereoImageReader

  puts "** motion estimate: "
  puts "[geom::print [$vmeEngine t3d]]"
  set T [[$vmeEngine t3d] getX]
  set TStdDev [[$vmeEngine t3d] getXStdDev]

  puts "t (m): [$T get 0] [$T get 1] [$T get 2]"
  puts "sigma: [$TStdDev get 0] [$TStdDev get 1] [$TStdDev get 2]"
  puts "r (deg): [jmath::radToDeg [$T get 3]] [jmath::radToDeg [$T get 4]] [jmath::radToDeg [$T get 5]]"
  puts "sigma: [jmath::radToDeg [$TStdDev get 3]] [jmath::radToDeg [$TStdDev get 4]] [jmath::radToDeg [$TStdDev get 5]]"

  puts "vme duration: [$vmeEngine getElapsedTime] ms"

  ::vme::delete_Engine $vmeEngine
    }

    proc batchEstimateMotion {indexFirstFrame indexLastFrame
            { indexStep 1}
            { scale 1.0 }
            { dataDir "." }
            { enableWeight "true" }
            { enableOutliers "true" }
            { enableUncertainty "true"}
            { minDisparity 0.5 } 
            { maxStereoDeltaV 1.0 }
            { nbMin3DPointPairs 3 }
            { removeOutliersKmax 5 } 
            { removeOutliersKmin 3 }
            { sigmaPix 0.5 } 
            { sigmaDisp 0.5 }
            { doDisplay true }
            { imagesSubDir "images" }
            { vmeSubDir "vme" }
            { configSubDir "config" }
        } {
  # batch motion estimate for a serie of stereo frames
  # this macro uses module stereo to correct stereo images pairs

  # DataLogger
  global dataLogger
  set dataLogger [::kernel::new_DataLogger "vme.dat"]

  # image reader
  set stereoImageReader [::image::new_StereoImageSequenceReader]
  $stereoImageReader setup [file join $dataDir $imagesSubDir] "image." ".l" ".r" ".tiff"

  # stereo bench model
  set sb [::image::new_StereoBench]
  $sb load [file join $dataDir $configSubDir "stereo.cal"]
  $sb applyScale $scale

  # vme engine
  global vmeEngine
  set vmeEngine [::vme::new_Engine $::geom::T3D_EULER $sb]
  ::image::delete_StereoBench $sb
  $vmeEngine setup $enableWeight $enableOutliers $enableUncertainty \
      $minDisparity $maxStereoDeltaV \
      $nbMin3DPointPairs \
      $removeOutliersKmax $removeOutliersKmin
  $vmeEngine setupUncertainty $sigmaPix $sigmaDisp

  $dataLogger addLoggable $vmeEngine
  $dataLogger addLoggable [$vmeEngine getStereoTrackingEngine]

  puts ""
  puts "** frame $indexFirstFrame **"

  # load images
  set imageFrame1Left  [$stereoImageReader loadLeft  $indexFirstFrame]
  set imageFrame1Right [$stereoImageReader loadRight $indexFirstFrame]

  # rectif
  set imagesFrame1_rect [::stereo::rectification_old $imageFrame1Left $imageFrame1Right [file join $dataDir $configSubDir "vecteurs"] $scale]
  set imageFrame1Left_rect [lindex $imagesFrame1_rect 0]
  set imageFrame1Right_rect [lindex $imagesFrame1_rect 1]

  # free memory
  ::image::delete_JfrImage $imageFrame1Left
  ::image::delete_JfrImage $imageFrame1Right

  # init vme
  $vmeEngine init $imageFrame1Left_rect $imageFrame1Right_rect

  if $doDisplay {
  }
  
  # free memory
  ::image::delete_JfrImage $imageFrame1Left_rect
  ::image::delete_JfrImage $imageFrame1Right_rect

  set index $indexFirstFrame
  incr index $indexStep

  for {} {$index <= $indexLastFrame} {incr index $indexStep} {

      puts ""
      puts "** frame $index **"

      # load images
      set imageFrame2Left  [$stereoImageReader loadLeft  $index]
      set imageFrame2Right [$stereoImageReader loadRight $index]

      # rectif
      set imagesFrame2_rect [::stereo::rectification_old $imageFrame2Left $imageFrame2Right [file join $dataDir $configSubDir "vecteurs"] $scale]
      set imageFrame2Left_rect [lindex $imagesFrame2_rect 0]
      set imageFrame2Right_rect [lindex $imagesFrame2_rect 1]

      # free memory
      ::image::delete_JfrImage $imageFrame2Left
      ::image::delete_JfrImage $imageFrame2Right

      # vme
      $vmeEngine estimateMotion $imageFrame2Left_rect $imageFrame2Right_rect $index

      $dataLogger log

      if $doDisplay {
      }

      # free memory
      ::image::delete_JfrImage $imageFrame2Left_rect
      ::image::delete_JfrImage $imageFrame2Right_rect

      # save t3d
      set from [expr $index - $indexStep]
      set to $index
      [$vmeEngine t3d] save [file join $dataDir $vmeSubDir "[format "%04d" $from]_to_[format "%04d" $to].t3d"]

      puts "** motion estimate: "
      puts "[geom::print [$vmeEngine t3d]]"
      set T [[$vmeEngine t3d] getX]
      set TStdDev [[$vmeEngine t3d] getXStdDev]

      puts "t (m): [$T get 0] [$T get 1] [$T get 2]"
      puts "sigma: [$TStdDev get 0] [$TStdDev get 1] [$TStdDev get 2]"
      puts "r (deg): [jmath::radToDeg [$T get 3]] [jmath::radToDeg [$T get 4]] [jmath::radToDeg [$T get 5]]"
      puts "sigma: [jmath::radToDeg [$TStdDev get 3]] [jmath::radToDeg [$TStdDev get 4]] [jmath::radToDeg [$TStdDev get 5]]"

  }

  # free memory
  ::image::delete_StereoImageSequenceReader $stereoImageReader
  ::vme::delete_Engine $vmeEngine
  ::kernel::delete_DataLogger $dataLogger
    }

}

package provide vme 0.2