engine.hpp

/* $Id$ */

#ifndef VME_ENGINE_HPP
#define VME_ENGINE_HPP

#include <list>

#include "kernel/jafarException.hpp"
#include "kernel/timingTools.hpp"
#include "kernel/dataLog.hpp"
#include "kernel/keyValueFile.hpp"

#include "jmath/jblas.hpp"

#include "geom/t3d.hpp"

#include "image/Image.hpp"
#include "camera/cameraPinhole.hpp"

#include "gfm/MatchingResult.hpp"
#include "gfm/EngineTracking.hpp"

namespace jafar {
  namespace vme {

    class Point3D {

    public:

      Point3D(double disparity_, jblas::vec3 const& x_);

      Point3D(double disparity_,
        jblas::vec3 const& x_,
        jblas::sym_mat const& xCov_);

      double disparity;

      jblas::vec3 x;
      jblas::sym_mat xCov;

    }; // class Point3D

    class Point3DPair {

    public:

      Point3DPair(Point3D const& first_, Point3D const& second_) :
  first(first_),
  second(second_)
      {}

      Point3D const& first;
      Point3D const& second;
      double weight;
      double error;

    }; // class Point3DPair

    class VmeEngineParams : public jafar::kernel::KeyValueFileLoad {
    public:

    VmeEngineParams(bool enableWeight_ = true,
        bool enableOutliers_ = true,
        bool enableUncertainty_ = true,
        double minDisparity_ = 0.5, double maxStereoDeltaV_ = 1.0, 
        std::size_t nbMin3DPointPairs_ = 3,
        int removeOutliersKmax_ = 4, int removeOutliersKmin_ = 3,
        double sigmaPix_ = 0.5, double sigmaDisp_ = 0.5) :
      enableWeight(enableWeight_),
      enableOutliers(enableOutliers_),
      enableUncertainty(enableUncertainty_),
      minDisparity(minDisparity_),
      maxStereoDeltaV(maxStereoDeltaV_),
      removeOutliersKmax(removeOutliersKmax_),
      removeOutliersKmin(removeOutliersKmin_),
      nbMin3DPointPairs(nbMin3DPointPairs_),
      sigmaPix(sigmaPix_),
      sigmaDisp(sigmaDisp_) 
    {}

      bool enableWeight;

      bool enableOutliers;

      bool enableUncertainty;

      double minDisparity;

      double maxStereoDeltaV;

      int removeOutliersKmax;

      int removeOutliersKmin;

      std::size_t nbMin3DPointPairs;

      double sigmaPix; 

      double sigmaDisp;

      void check() const {
  JFR_TRACE_BEGIN;
  JFR_CHECK_PARAM(minDisparity > 0.0, minDisparity);
  JFR_CHECK_PARAM(maxStereoDeltaV > 0, maxStereoDeltaV);
  JFR_CHECK_PARAM(nbMin3DPointPairs >= 3, nbMin3DPointPairs);
  JFR_CHECK_PARAM(removeOutliersKmin >= 3, removeOutliersKmin);
  JFR_CHECK_PARAM(removeOutliersKmax >= removeOutliersKmin, removeOutliersKmax);
  JFR_CHECK_PARAM(sigmaPix >= 0.0, sigmaPix);
  JFR_CHECK_PARAM(sigmaDisp >= 0.0, sigmaDisp);
  JFR_TRACE_END("VmeEngineParams::check");
      };

      virtual void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
      
      }; // class VmeEngineParams

    class VmeEngine : public jafar::kernel::DataLoggable {
      
    private:

      VmeEngineParams params;

      // vme working variables

      double errorMean;
      double errorSigma;

      gfm::MatchingResult m_trackingResult;
      gfm::MatchingResult m_stereoResult;

      std::list<Point3D> point3DListPrev;

      std::vector<Point3D*> point3DIndexPrev;
      
      std::list<Point3D> point3DListCur;

      std::vector<Point3D*> point3DIndexCur;

      typedef std::list<Point3DPair> Point3DPairList;
      Point3DPairList me3DPointPairs;

      std::size_t sizeMe3DPointPairs;

      double sumMe3DPointPairsWeights;

      jblas::mat33 R;

      jblas::vec3 t;

//       /// minimisation residual
//       double epsilon;

      jblas::diag_mat uvdCov;

      geom::T3D* m_t3d;

      //geom::T3D& t3d() {return *m_t3d;}

      camera::StereoBench m_stereoBench;

      gfm::EngineStereoTracking m_engineStereoTracking;

      // logging variables
      unsigned int frameIndex;

      int nbOutliers;

      long elapsedTime;
      bool success;

      void fillPoint3DList(const jafar::gfm::MatchingResult& stereoResult,
         std::list<Point3D>& point3DList,
         std::vector<Point3D*>& point3DIndex);
      void estimateMotion(bool doComputeErrors);
      bool removeOutliers(int k);
      void estimateMotionUncertainty();

    protected:

      virtual void writeLogHeader(jafar::kernel::DataLogger& log) const;
      virtual void writeLogData(jafar::kernel::DataLogger& log) const;

    public:

      VmeEngine(jafar::geom::T3D::Type t3dType, jafar::camera::StereoBench const& stereoBench);

      ~VmeEngine();
      
      void setStereoBench( jafar::camera::StereoBench const& stereoBench )
      {
        m_stereoBench = stereoBench;
      }

      // added for THALES_TAROT
      std::size_t nbTracked3DPoint() {
        return sizeMe3DPointPairs;
      }
      int nb3DPointsOutliers() {
        return nbOutliers;
      }
      // end added

      void setParams(VmeEngineParams const& params_) {
  params_.check();
  params = params_;
  uvdCov(0,0) = params.sigmaPix*params.sigmaPix;
  uvdCov(1,1) = params.sigmaPix*params.sigmaPix;
  uvdCov(2,2) = params.sigmaDisp*params.sigmaDisp;
      }

      const jafar::gfm::MatchingResult& getStereoResult() const {return m_stereoResult;}
      const jafar::gfm::MatchingResult& getTrackResult() const {return m_trackingResult;}

      jafar::geom::T3D& t3d() {return *m_t3d;}

      long getElapsedTime() const {return elapsedTime;}

      void init(const jafar::image::Image& imageLeft,
                const jafar::image::Image& imageRight,
                unsigned int frameIndex_ = 0);

      void init(const jafar::gfm::MatchingResult& stereoResult,
    unsigned int frameIndex_ = 0);

      void estimateMotion(const jafar::image::Image& imageLeft, 
                          const jafar::image::Image& imageRight,
                          unsigned int frameIndex_ = 0);

      void estimateMotion(const jafar::gfm::MatchingResult& trackingResult,
        const jafar::gfm::MatchingResult& stereoResult,
        unsigned int frameIndex_ = 0);

      int count3DPointPairs() const {
        return sizeMe3DPointPairs;
      }
    }; // class VmeEngine

  } // namespace vme
} // namespace jafar

#endif // VME_ENGINE_HPP