
/// Maestría en Análisis y Procesamiento de Imágenes
///
/// Curso: Visión Robótica
///
/// Trabajo Práctico Nro. 1
///
///   1) Visualizar las correspondencias que dan consenso (inliers) a
///   la homografía estimada. Se debe generar un nuevo "tile" y
///   dibujar sobre el mismo *solamente* los pares de correspondencias
///   para los que cumple: ||p1-H21*p2||<5.0. Calcular el porcentaje
///   de inliers respecto del número total de correspondencias
///   (matches) iniciales.
///
///   2) Considérese la imagen "img1_dark.ppm" para el caso de la
///   variable im2_rgb. Esta imagen corresponde a una vesión "oscura"
///   de "img1.ppm". El matching entre estas dos imágenes falla debido
///   a las diferencias en las condiciones de iluminación
///   existentes. Se observó que la ecualización de los histogramas es
///   una posible solución al problema. En lugar de ecualización, otra
///   posibilidad es "robustecer" la medida de similitud utilizada en
///   el cómputo de correspondencias haciendo que los patches estén
///   normalizados en la media (valor medio de gris igual a cero). Se
///   pide implementar esta operación substrayendo la media de patch_i
///   y patch_j en la función computeMatches() antes del cálculo de la
///   "suma de diferencias cuadradas" (SSD, por sus siglas en inglés).
///
///   3) Considérese nuevamente las imágenes "img1.ppm" y
///   "img2.ppm". En el caso del cómputo de correspondencias, una
///   alternativa a la SSD ya implementada es la utilización de "suma
///   de diferencias absolutas" (SAD, por sus siglas en inglés). Se
///   pide implementar dicha función y comparar su rendimiento
///   respecto de SSD. Para ello se deberá evaluar el número de
///   correspondencias "inlier" en función del valor utilizado como
///   umbral (la variable "ssd_threshold") en amos casos.
///
/// (*) "img1.ppm" y "img2.ppm" son una versión escalada de las
/// correspondientes a la secuencia "Grafiti", disponible en:
/// http://www.robots.ox.ac.uk/~vgg/research/affine/det_eval_files/graf.tar.gz
///
/// Alumno:
///
/// Noviembre 2011       

#include <cv.h>
#include <highgui.h>
#include <iostream>

// estructura para representar puntos correspondientes
struct pointPair{
  cv::Point p1;
  cv::Point p2;
};

// typedefs útiles
typedef std::vector<pointPair> vPointPair;
typedef std::vector<cv::Point> vPoint;

// prototipo de funciones
cv::Mat tile1x2( cv::Mat &im1, cv::Mat &im2 );
vPoint localMaxima3x3( cv::Mat &im, float thres, int border );
vPointPair computeMatches( cv::Mat &im1, vPoint &c1, cv::Mat &im2, vPoint &c2, float thr, int N );

// main
int main( int argc, char* argv[] )
{
  (void)argc;
  (void)argv;

  //---------------------------------
  //  Leer imágenes
  //---------------------------------

  // lee imágenes del disco
  // cv::Mat im1_rgb = cv::imread( "images/scene1.row3.col1.ppm", -1 );
  // cv::Mat im2_rgb = cv::imread( "images/scene1.row3.col3.ppm", -1 );
  cv::Mat im1_rgb = cv::imread( "images/img1.ppm", -1 );
  cv::Mat im2_rgb = cv::imread( "images/img2.ppm", -1 );

  // // cambios de iluminación
  // cv::Mat im2_rgb = cv::imread( "images/img2_dark.ppm", -1 );

  // // rotaciones
  // cv::Mat im2_rgb = cv::imread( "images/img1_rot15.ppm", -1 );
  // cv::Mat im2_rgb = cv::imread( "images/img1_rot30.ppm", -1 );
  
  // convierte las imágenes a escala de grises
  cv::Mat im1_gray, im2_gray;
  cv::cvtColor( im1_rgb, im1_gray, CV_RGB2GRAY );
  cv::cvtColor( im2_rgb, im2_gray, CV_RGB2GRAY );  

  // cv::Mat eq;
  // eq = tile1x2( im1_gray, im2_gray );
  // cv::imshow( "Im", eq );
  // cv::equalizeHist( im1_gray, im1_gray );
  // cv::equalizeHist( im2_gray, im2_gray );
  // eq = tile1x2( im1_gray, im2_gray );
  // cv::imshow( "Im eq", eq );

  //convierte las imágenes a punto flotente
  cv::Mat im1, im2;
  im1_gray.convertTo( im1, CV_32F );
  im2_gray.convertTo( im2, CV_32F );

  // [0,255] -> [0,1]
  im1 *= (1.0/255.0);
  im2 *= (1.0/255.0);

  // visualiza imágenes RGB
  cv::Mat tile;
  tile = tile1x2( im1_rgb, im2_rgb );
  cv::imshow( "Imágenes", tile );

  //---------------------------------
  //  Extracción de características
  //---------------------------------

  // Parámetros del detector de Harris
  int block = 3;
  double k = 0.04;
  double thres = 1e-3;

  // Medida de Harris (cornerness map) para cada imagen
  cv::Mat cm1( im1.size(), CV_32FC1 );
  cv::cornerHarris( im1, cm1, block, 3, k );

  cv::Mat cm2( im2.size(), CV_32FC1 );
  cv::cornerHarris( im2, cm2, block, 3, k );

  // Detección de máximos locales (interest points) a partir de la medida de Harris
  vPoint ip1 = localMaxima3x3( cm1, thres, 15 );
  std::cout << " >> " << ip1.size() << " puntos extraídos de la imagen 1" << std::endl;

  vPoint ip2 = localMaxima3x3( cm2, thres, 15 );
  std::cout << " >> " << ip2.size() << " puntos extraídos de la imagen 2" << std::endl;

  // Visualización de los puntos extraídos
  tile = tile1x2( im1_rgb, im2_rgb );
  for( int i=0; i<(int)ip1.size(); ++i ) 
  {
    cv::circle( tile, ip1[i], 2, cv::Scalar(0,255,0), 2, 8, 0 );  
  }
  for( int i=0; i<(int)ip2.size(); ++i ) 
  {
    cv::circle( tile, cv::Point(ip2[i].x+im1.cols,ip2[i].y), 2, cv::Scalar(0,255,0), 2, 8, 0 );  
  }
  cv::imshow( "Puntos de interés", tile );

  //---------------------------------
  //  Matching
  //---------------------------------

  float ssd_threshold = 1e2;
  int patch_size = 15;

  vPointPair matches = computeMatches( im1, ip1, im2, ip2, ssd_threshold, patch_size );  

  // visualización
  std::cout << " >> " << matches.size() << " correspondencias" << std::endl;
  for( int i=0; i<(int)matches.size(); ++i )
  {
    cv::Point p1 = matches[i].p1;
    cv::Point p2 = cv::Point( matches[i].p2.x+im1.cols, matches[i].p2.y );
    cv::line( tile, p1, p2, cv::Scalar(255,0,0) );
  }
  cv::imshow( "Correspondencias", tile );

  //---------------------------------
  //  Cálculo de homografía
  //---------------------------------

  // listas -> matrices
  cv::Mat m_ip1(matches.size(),2,CV_32FC1);
  cv::Mat m_ip2(matches.size(),2,CV_32FC1);
  for( int i=0; i<(int)matches.size(); ++i ) 
  {
    m_ip1.at<float>(i,0) = matches[i].p1.x;
    m_ip1.at<float>(i,1) = matches[i].p1.y;
    m_ip2.at<float>(i,0) = matches[i].p2.x;
    m_ip2.at<float>(i,1) = matches[i].p2.y;
  }
  
  cv::Mat H12 = cv::findHomography( m_ip1, m_ip2, CV_RANSAC, 5.0 );

  std::cout << "Homografía estimada:" << std::endl;
  std::cout << H12 << std::endl;

  //---------------------------------
  //  Rectificación de im2
  //---------------------------------

  double ox = im1.cols/2.0;
  double oy = im1.rows/2.0;

  cv::Mat Ho( 3, 3, H12.type(), cv::Scalar(0) );
  Ho.at<double>(0,0) = 1.0; 
  Ho.at<double>(0,2) = ox;
  Ho.at<double>(1,1) = 1.0;
  Ho.at<double>(1,2) = oy;
  Ho.at<double>(2,2) = 1.0;  

  cv::Mat H21 = H12.inv();

  // H21 = Ho * H21;

  cv::Mat im1_big( 2*im1.rows, 2*im1.cols, im1_rgb.type(), cv::Scalar(0) );
  cv::Mat im1_roi( im1_big, cv::Rect(0,0,im1.cols,im1.rows) );
  im1_rgb.copyTo(im1_roi);

  cv::Mat im2_big = im1_big.clone();
  cv::Mat im2_roi( im2_big, cv::Rect(0,0,im2.cols,im2.rows) );
  im2_rgb.copyTo(im2_roi);

  cv::Mat im1r, im2r;
  cv::warpPerspective( im1_big, im1r, Ho, cv::Size(im1_big.cols,im1_big.rows) );
  cv::warpPerspective( im2_big, im2r, Ho*H21, cv::Size(im1_big.cols,im1_big.rows) );

  // visualización
  float alpha = 0.5;
  cv::Mat im = alpha*im1r + (1.0-alpha)*im2r;
  cv::imshow("... llevando im2 a im1",im);

  // espera que se presiones una tecla
  std::cout << "\nPresione cualquier tecla para salir..." << std::endl;
  cv::waitKey(0);
  return 0;
}

//------------------------------------------------------------------------------------

// Función que genera una imagen a partir de otras dos (lado a lado)
//
// Entrada:
//   im1: imagen 1 de alto H y ancho W1
//   im2: imagen 2 de alto H y ancho W2
//
// Salida:
//   imagen de alto H y ancho W1+W2

cv::Mat 
tile1x2( cv::Mat &im1, cv::Mat &im2 )
{
  assert( im1.rows==im2.rows && im1.type()==im2.type() );

  int H = im1.rows;
  int W1 = im1.cols;
  int W2 = im2.cols;

  cv::Mat tile( H, W1+W2, im1.type() );

  cv::Mat r1 = cv::Mat( tile, cv::Rect(0,0,W1,H) );
  im1.copyTo( r1 );

  cv::Mat r2 = cv::Mat( tile, cv::Rect(W1,0,W2,H) );
  im2.copyTo( r2 );

  return tile;
}


// Función que detecta máximos locales en una vecindad de 3x3 pixels.
// Se utiliza la sigiente denominación (p0 es el pixel central):
//
//     p1 p2 p3
//     p4    p5
//     p6 p7 p8
//
// Entrada:
//   im: imagen
//   thres: umbral de detección
//   border: borde 


vPoint 
localMaxima3x3( cv::Mat &im, float thres, int border )
{
  vPoint points;
  for( int i=border; i<im.rows-border; ++i ) 
  {
    for( int j=border; j<im.cols-border; ++j ) 
    {
      // la respuesta es lo suficientemente fuerte?
      float p0 = im.at<float>(i,j);
      if( p0<thres )
        continue;      

      // es un máximo local?
      float p1 = im.at<float>(i-1,j-1);
      if( p1>p0 )
        continue;

      float p2 = im.at<float>(i-1,j);
      if( p2>p0 )
        continue;

      float p3 = im.at<float>(i-1,j+1);
      if( p3>p0 )
        continue;

      float p4 = im.at<float>(i,j-1);
      if( p4>p0 )
        continue;

      float p5 = im.at<float>(i,j+1);
      if( p5>p0 )
        continue;

      float p6 = im.at<float>(i+1,j-1);
      if( p6>p0 )
        continue;

      float p7 = im.at<float>(i+1,j);
      if( p7>p0 )
        continue;

      float p8 = im.at<float>(i+1,j+1);
      if( p8>p0 )
        continue;

      points.push_back( cv::Point(j,i) );
    } 
  }
  
  return points;
}


// Función para el computo de correspondencias
//
// Argumentos:
//   im1: imagen 1
//   ip1: lista de puntos correspondientes a la imagen 1
//   im1: imagen 2
//   ip2: lista de puntos correspondientes a la imagen 2
//   thr: matching threshold
//   N: lado (impar) de la ventana extraída alrededor de cada punto (NxN)
//
// Retorna:
//   lista de pares de correspondencias

vPointPair 
computeMatches( cv::Mat &im1, vPoint &ip1, cv::Mat &im2, vPoint &ip2, float thr, int N )
{
  assert(N%2);
  int hN = N/2; // "half N"

  vPointPair matches;

  cv::Mat patch_i, patch_j, aux;

  for( int i=0; i<(int)ip1.size(); ++i ) 
  {    
    // obtener ventana centrada en ( ip1[i].x, ip1[i].y ) 
    aux = cv::Mat( im1, cv::Rect( ip1[i].x-hN, ip1[i].y-hN, N, N ) );
    patch_i = aux.clone();

    float best_SSD=1e12;
    int best_SSD_index=-1;

    for( int j=0; j<(int)ip2.size(); ++j ) 
    {
      // obtener ventana centrada en ( ip2[j].x, ip2[j].y ) 
      aux = cv::Mat( im2, cv::Rect( ip2[j].x-hN, ip2[j].y-hN, N, N ) );
      patch_j = aux.clone();

      // calcula "distancia" entre patches
      cv::Mat SD;
      cv::multiply( patch_i-patch_j, patch_i-patch_j, SD );           
      float SSD = (cv::sum(SD)).val[0]/float(N*N);

      if( SSD < best_SSD )
      {
        best_SSD = SSD;
        best_SSD_index = j;
      }

      // if( SSD<thr )
      // {
      //   pointPair pair;
      //   pair.p1 = ip1[i];
      //   pair.p2 = ip2[j];
      //   matches.push_back( pair );
      // }
    }

    if( best_SSD<thr )
    {
      pointPair pair;
      pair.p1 = ip1[i];
      pair.p2 = ip2[best_SSD_index];
      matches.push_back( pair );
    }

  }

  return matches;
}
