vpPlanarObjectDetector.cpp

/****************************************************************************
 *
 * $Id: vpPlanarObjectDetector.cpp 4632 2014-02-03 17:06:40Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Planar surface detection tool.
 *
 * Authors:
 * Romain Tallonneau
 *
 *****************************************************************************/

#include <visp/vpPlanarObjectDetector.h>

#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0

#include <visp/vpImageConvert.h>
#include <visp/vpException.h>
#include <visp/vpImagePoint.h>
#include <visp/vpDisplay.h>
#include <visp/vpDisplayX.h>
#include <visp/vpColor.h>
#include <visp/vpImageTools.h>

#include <vector>
#include <iostream>

vpPlanarObjectDetector::vpPlanarObjectDetector()
  : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
    refImagePoints(), minNbMatching(10)
{
}

vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, const std::string& _objectName)
  : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
    refImagePoints(), minNbMatching(10)
{
  load(_dataFile, _objectName);
}

void
vpPlanarObjectDetector::init()
{
}

vpPlanarObjectDetector::~vpPlanarObjectDetector() 
{

}

void 
vpPlanarObjectDetector::computeRoi(vpImagePoint* ip, const unsigned int nbpt)
{
  if(nbpt < 3){
    throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
  }
  
  std::vector < vpImagePoint > ptsx(nbpt);
  std::vector < vpImagePoint > ptsy(nbpt);
  for(unsigned int i=0; i<nbpt; i++){
    ptsx[i] = ptsy[i] = ip[i];
  }
  
  for(unsigned int i=0; i<nbpt; i++){
    for(unsigned int j=0; j<nbpt-1; j++){
      if(ptsx[j].get_j() > ptsx[j+1].get_j()){
        double tmp = ptsx[j+1].get_j();
        ptsx[j+1].set_j(ptsx[j].get_j());
        ptsx[j].set_j(tmp);
      }
    }
  }
  for(unsigned int i=0; i<nbpt; i++){
    for(unsigned int j=0; j<nbpt-1; j++){
      if(ptsy[j].get_i() > ptsy[j+1].get_i()){
        double tmp = ptsy[j+1].get_i();
        ptsy[j+1].set_i(ptsy[j].get_i());
        ptsy[j].set_i(tmp);
      }
    }
  }
    
}


unsigned int
vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
{
  modelROI.x = 0;
  modelROI.y = 0;
  modelROI.width = (int)_I.getWidth();
  modelROI.height = (int)_I.getHeight();  

  initialiseRefCorners(modelROI);  
  
  return fern.buildReference(_I);
}


unsigned int
vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
               const vpImagePoint &_iP,
               unsigned int _height, unsigned int _width)
{
  unsigned int res = fern.buildReference(_I, _iP, _height, _width);
  modelROI.x = (int)_iP.get_u();
  modelROI.y = (int)_iP.get_v();
  modelROI.width = (int)_width;
  modelROI.height = (int)_height; 

  initialiseRefCorners(modelROI);  
  
  return res;
}



unsigned int 
vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
               const vpRect _rectangle)
{
  unsigned int res = fern.buildReference(_I, _rectangle);
  
  vpImagePoint iP = _rectangle.getTopLeft();
  
  modelROI.x = (int)iP.get_u();
  modelROI.y = (int)iP.get_v();
  modelROI.width = (int)_rectangle.getWidth();
  modelROI.height = (int)_rectangle.getHeight(); 

  initialiseRefCorners(modelROI);  
  
  return res;
}


bool 
vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
{
  fern.matchPoint(I);
  
  /* compute homography */
  std::vector<cv::Point2f> refPts = fern.getRefPt();
  std::vector<cv::Point2f> curPts = fern.getCurPt();
  
  for(unsigned int i=0; i<refPts.size(); ++i){
    refPts[i].x += modelROI.x;
    refPts[i].y += modelROI.y;
  }
  for(unsigned int i=0; i<curPts.size(); ++i){
    curPts[i].x += modelROI.x;
    curPts[i].y += modelROI.y;
  }
  
  if(curPts.size() < 4){
    for (unsigned int i = 0; i < 3; i += 1){
      for (unsigned int j = 0; j < 3; j += 1){
        if(i == j){
          homography[i][j] = 1;
        }
        else{
          homography[i][j] = 0;
        }
      }
    }
    return false;
  } 

  /* part of code from OpenCV planarObjectDetector */
  std::vector<unsigned char> mask;
  H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
  
  if( H.data )
  {
    const cv::Mat_<double>& H_tmp = H;
    dst_corners.resize(4);
    for(unsigned int i = 0; i < 4; i++ )
    {
        cv::Point2f pt = ref_corners[i];

        double w = 1./(H_tmp(2,0)*pt.x + H_tmp(2,1)*pt.y + H_tmp(2,2));
        dst_corners[i] = cv::Point2f((float)((H_tmp(0,0)*pt.x + H_tmp(0,1)*pt.y + H_tmp(0,2))*w),
                             (float)((H_tmp(1,0)*pt.x + H_tmp(1,1)*pt.y + H_tmp(1,2))*w));
    }
    
    double* ptr = (double*)H_tmp.data;
    for(unsigned int i=0; i<9; i++){
      this->homography[(unsigned int)(i/3)][i%3] = *(ptr++);
    }
    isCorrect = true;
  }
  else{
    isCorrect = false;
  }
  
  
  currentImagePoints.resize(0);
  refImagePoints.resize(0);
  for (unsigned int i = 0; i < mask.size(); i += 1){
    if(mask[i] != 0){
      vpImagePoint ip;
      ip.set_i(curPts[i].y);
      ip.set_j(curPts[i].x);
      currentImagePoints.push_back(ip);
      ip.set_i(refPts[i].y);
      ip.set_j(refPts[i].x);
      refImagePoints.push_back(ip);
    }
  }
  
  if(currentImagePoints.size() < minNbMatching){
    isCorrect = false;
  }

  return isCorrect;
}



bool 
vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I,
           const vpImagePoint &iP, const unsigned int height, const unsigned int width)
{
  if((iP.get_i()+height) >= I.getHeight()
     || (iP.get_j()+width) >= I.getWidth()) {
    vpTRACE("Bad size for the subimage");
    throw(vpException(vpImageException::notInTheImage ,
              "Bad size for the subimage"));
  }

  vpImage<unsigned char> subImage;

  vpImageTools::createSubImage(I,
                   (unsigned int)iP.get_i(),
                   (unsigned int)iP.get_j(),
                   height, width, subImage);

  return this->matchPoint(subImage);
}

bool 
vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle)
{
  vpImagePoint iP;
  iP.set_i(rectangle.getTop());
  iP.set_j(rectangle.getLeft());
  return (this->matchPoint(I, iP,
               (unsigned int)rectangle.getHeight(),
               (unsigned int)rectangle.getWidth()));
}


void 
vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
{
  for(unsigned int i=0; i<dst_corners.size(); i++){
    vpImagePoint ip1(
      dst_corners[i].y - modelROI.y, 
      dst_corners[i].x - modelROI.x);
    vpImagePoint ip2(
      dst_corners[(i+1)%dst_corners.size()].y - modelROI.y, 
      dst_corners[(i+1)%dst_corners.size()].x - modelROI.x);
    vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
  }
  
  if(displayKpts){
    for(unsigned int i=0; i<currentImagePoints.size(); ++i){
      vpImagePoint ip(
        currentImagePoints[i].get_i() - modelROI.y,
        currentImagePoints[i].get_j() - modelROI.x);
      vpDisplay::displayCross(I, ip, 5, vpColor::red);
    }
  }
}


void 
vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref,
     vpImage<unsigned char> &Icurrent, bool displayKpts)
{
  display(Icurrent, displayKpts);

  if(displayKpts){
    for(unsigned int i=0; i<refImagePoints.size(); ++i){
      vpDisplay::displayCross(Iref, refImagePoints[i], 5, vpColor::green);
    }
  }
}


void
vpPlanarObjectDetector::load(const std::string& dataFilename, const std::string& objName)
{
  fern.load(dataFilename, objName);
  modelROI = fern.getModelROI();
  initialiseRefCorners(modelROI);
}


void
vpPlanarObjectDetector::recordDetector(const std::string& objectName, const std::string& dataFile )
{
  fern.record(objectName, dataFile);
}



std::vector<vpImagePoint> 
vpPlanarObjectDetector::getDetectedCorners() const{
  vpImagePoint ip;
  std::vector <vpImagePoint> corners;
  corners.clear();
  for(unsigned int i=0; i<dst_corners.size(); i++){
    ip.set_uv( dst_corners[i].x, dst_corners[i].y);
    corners.push_back(ip);
  }

  return corners;
}

void 
vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect& _modelROI)
{
  cv::Point2f ip;
  
  ip.y = (float)_modelROI.y; 
  ip.x = (float)_modelROI.x;
  ref_corners.push_back(ip);
  
  ip.y = (float)(_modelROI.y+_modelROI.height); 
  ip.x = (float)_modelROI.x;  
  ref_corners.push_back(ip);
  
  ip.y = (float)(_modelROI.y+_modelROI.height); 
  ip.x = (float)(_modelROI.x+_modelROI.width);  
  ref_corners.push_back(ip);  
  
  ip.y = (float)_modelROI.y; 
  ip.x = (float)(_modelROI.x+_modelROI.width);
  ref_corners.push_back(ip);
}


void 
vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint& _imPoint)
{
  if(_i >= refImagePoints.size()){
    throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
  }
  _imPoint = refImagePoints[_i];
}

void 
vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint)
{
//  fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
  if(_index >= currentImagePoints.size()){
    throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
  }

  _referencePoint = refImagePoints[_index];
  _currentPoint = currentImagePoints[_index];
}

#endif