vpPlanarObjectDetector.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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.
 */
 
#include <visp3/vision/vpPlanarObjectDetector.h>
 
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408) &&                                                                          \
    (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.4.8 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 around to avoid warning:
// libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
void dummy_vpPlanarObjectDetector() { };
#endif