vpPlanarObjectDetector.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 <visp3/vision/vpPlanarObjectDetector.h>

#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) &&                                                                          \
    (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0

#include <visp3/core/vpColor.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpImageTools.h>

#include <iostream>
#include <vector>

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, 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,
                                        unsigned int height, 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::crop(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];
}

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning:
// libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
void dummy_vpPlanarObjectDetector(){};
#endif