trv_tests.hpp

#ifndef TRV_TESTS_HPP
#define TRV_TESTS_HPP

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include <boost/numeric/ublas/storage.hpp> // to allow ranges


#include "traversability/traversability.hpp"
#include "traversability/trv_rotations.hpp"
#include "traversability/trv_util.hpp"
#include "traversability/trv_homography.hpp"
#include "traversability/trv_dataProvider.hpp"

#include "datareader/PositionReader.hpp"
#include "datareader/ImageReader.hpp"

#include "geom/t3d.hpp"
#include "geom/t3dEuler.hpp"

#include <image/Image.hpp>  // images
#include <fstream>          // to write data files
#include <string>

#include "fdetect/DetectionResult.hpp"  // for testZNCCData
#include "gfm/MatchingResult.hpp"
#include "fdetect/InterestFeature.hpp"

namespace jafar {
  namespace traversability {
    
    // test routines
    void testRotations ( double psi, double theta, double phi );
    jafar::image::Image testHomographyGeom ( int idx, int stepSize );
    void testZNCC ( const jafar::image::Image &img0, const int centerX, const int centerY,
                    int winHalfwidth, int winHalfHeight, int halfnos, double minShift, double maxShift );
    void testZNCCData ( const jafar::image::Image &img0, const jafar::image::Image &img1,
                        const jafar::gfm::MatchingResult &rm, int winHalfwidth, int winHalfHeight, int fileIndex );
    
    // utilities
    int elementwiseMatrixComparison (jblas::mat33 A,jblas::mat33 B );
//    void printMat33 ( jblas::mat33 A );
//    void printQuat ( jblas::vec4 q);
//    void printVec6 ( jblas::vec6 x);
    
    jafar::datareader::PositionReader* initPositionReader ( FileInfoDataSet fileInfo );
    jafar::datareader::ImageReader* initImageReader ( FileInfoDataSet fileInfo );
    jblas::vec6 getPoseDataFromSource ( jafar::datareader::PositionReader* posReader, int idx );
    CameraPoseParameters getPoseDataFromSourceStr ( jafar::datareader::PositionReader* posReader, int idx );
    MatchesContainer readMatches ( std::string filename );
    
    
    // image related tasks
    void calcMaskImage ( const jafar::image::Image &img0, const jafar::image::Image &img1, jafar::image::Image &img2 );
    void calcMeanImage ( const jafar::image::Image &img0, const jafar::image::Image &img1, jafar::image::Image &img2 );
    void calcDiffImage ( const jafar::image::Image &img0, const jafar::image::Image &img1, jafar::image::Image &img2 );
    void moveImage ( const jafar::image::Image &img0, jafar::image::Image &img1, const double dx, const double dy);
    void calcZnccImage ( const jafar::image::Image &img0, const jafar::image::Image &img1, jafar::image::Image &imgZncc,
                         const int winHalfWidth, const int winHalfHeight, const int threshold = 0);
    void calcZnccImageO ( const jafar::image::Image &im0, const jafar::image::Image &im1, jafar::image::Image &imZncc,
                          const int winHalfWidth, const int winHalfHeight, const int threshold = 0);
    void calcZnccImageOO ( const jafar::image::Image &im0, const jafar::image::Image &im1,
                           const jafar::image::Image &maskCFoV, jafar::image::Image &imZncc,
                           const int winHalfWidth, const int winHalfHeight, const int threshold = 0);
    inline double pow2(double a) { return a * a; }
    
    
    // TODO: remove this function.
    void nastyDesaturation ( jafar::image::Image &img );
    
    // homography estimation and application (older)
    CameraDeltaParameters calcCameraDeltaParameters ( CameraPoseParameters camPose0,
        CameraPoseParameters camPose1 );
    jblas::mat33 calcHomographyGeom ( const CameraDeltaParameters &dCam, const CameraIntrinsicParameters &internals,
                                      const PlaneParameters &plane );
    void applyHomography( const jblas::mat33 &H_C0C1, const jafar::image::Image &img0, jafar::image::Image &img1 );
    void estimateHomography ( const jafar::gfm::MatchingResult &rm, jblas::mat33& H );
    void calcA_i ( const fdetect::InterestFeature* x0, const fdetect::InterestFeature* x1, jblas::mat &A_i );
    void calcJ_i( const fdetect::InterestFeature* x0, const fdetect::InterestFeature* x1, jblas::mat &J_i );
    
    // homography estimation (newer)
    void matchRepartition ( MatchesContainer &mc, const int m, const int n, const int width, const int height );
    
    
    // minimale baseline "management"
    void calcExpectedDisparity(int u0, int v0, const jblas::mat33 &H_C1C0,
                               const CameraIntrinsicParameters &internals,
                               const CameraDeltaParameters &deltaC01, const jafar::image::Image &mask1);
    
    // ugly test ugly test
    void calcDensificator0 ( const jafar::image::Image &im0, const jafar::image::Image &im1,
                             const jafar::image::Image &maskCFoV0, jafar::image::Image &imZncc, 
               const int winHalfWidth, const int winHalfHeight);
    
    void calcHomographyDecomposition ( const jblas::mat33 &H_C1C0, const jblas::mat33 K,
                                       jblas::mat33 &R_C1C0, jblas::vec3 &t_C1, jblas::vec3 &n_C0 );  
  }
}

#endif