Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpPlanarObjectDetector.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Planar surface detection tool.
33  *
34  * Authors:
35  * Romain Tallonneau
36  *
37  *****************************************************************************/
38 
39 #include <visp3/vision/vpPlanarObjectDetector.h>
40 
41 #if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \
42  (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
43 
44 #include <visp3/core/vpColor.h>
45 #include <visp3/core/vpDisplay.h>
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpImageConvert.h>
48 #include <visp3/core/vpImagePoint.h>
49 #include <visp3/core/vpImageTools.h>
50 
51 #include <iostream>
52 #include <vector>
53 
59 vpPlanarObjectDetector::vpPlanarObjectDetector()
60  : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
61  refImagePoints(), minNbMatching(10)
62 {
63 }
64 
73 vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string &_dataFile, const std::string &_objectName)
74  : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
75  refImagePoints(), minNbMatching(10)
76 {
77  load(_dataFile, _objectName);
78 }
79 
83 void vpPlanarObjectDetector::init() {}
84 
90 vpPlanarObjectDetector::~vpPlanarObjectDetector() {}
91 
99 void vpPlanarObjectDetector::computeRoi(vpImagePoint *ip, const unsigned int nbpt)
100 {
101  if (nbpt < 3) {
102  throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
103  }
104 
105  std::vector<vpImagePoint> ptsx(nbpt);
106  std::vector<vpImagePoint> ptsy(nbpt);
107  for (unsigned int i = 0; i < nbpt; i++) {
108  ptsx[i] = ptsy[i] = ip[i];
109  }
110 
111  for (unsigned int i = 0; i < nbpt; i++) {
112  for (unsigned int j = 0; j < nbpt - 1; j++) {
113  if (ptsx[j].get_j() > ptsx[j + 1].get_j()) {
114  double tmp = ptsx[j + 1].get_j();
115  ptsx[j + 1].set_j(ptsx[j].get_j());
116  ptsx[j].set_j(tmp);
117  }
118  }
119  }
120  for (unsigned int i = 0; i < nbpt; i++) {
121  for (unsigned int j = 0; j < nbpt - 1; j++) {
122  if (ptsy[j].get_i() > ptsy[j + 1].get_i()) {
123  double tmp = ptsy[j + 1].get_i();
124  ptsy[j + 1].set_i(ptsy[j].get_i());
125  ptsy[j].set_i(tmp);
126  }
127  }
128  }
129 }
130 
141 unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
142 {
143  modelROI.x = 0;
144  modelROI.y = 0;
145  modelROI.width = (int)_I.getWidth();
146  modelROI.height = (int)_I.getHeight();
147 
148  initialiseRefCorners(modelROI);
149 
150  return fern.buildReference(_I);
151 }
152 
166 unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I, const vpImagePoint &_iP,
167  unsigned int _height, unsigned int _width)
168 {
169  unsigned int res = fern.buildReference(_I, _iP, _height, _width);
170  modelROI.x = (int)_iP.get_u();
171  modelROI.y = (int)_iP.get_v();
172  modelROI.width = (int)_width;
173  modelROI.height = (int)_height;
174 
175  initialiseRefCorners(modelROI);
176 
177  return res;
178 }
179 
190 unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I, const vpRect &_rectangle)
191 {
192  unsigned int res = fern.buildReference(_I, _rectangle);
193 
194  vpImagePoint iP = _rectangle.getTopLeft();
195 
196  modelROI.x = (int)iP.get_u();
197  modelROI.y = (int)iP.get_v();
198  modelROI.width = (int)_rectangle.getWidth();
199  modelROI.height = (int)_rectangle.getHeight();
200 
201  initialiseRefCorners(modelROI);
202 
203  return res;
204 }
205 
216 bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
217 {
218  fern.matchPoint(I);
219 
220  /* compute homography */
221  std::vector<cv::Point2f> refPts = fern.getRefPt();
222  std::vector<cv::Point2f> curPts = fern.getCurPt();
223 
224  for (unsigned int i = 0; i < refPts.size(); ++i) {
225  refPts[i].x += modelROI.x;
226  refPts[i].y += modelROI.y;
227  }
228  for (unsigned int i = 0; i < curPts.size(); ++i) {
229  curPts[i].x += modelROI.x;
230  curPts[i].y += modelROI.y;
231  }
232 
233  if (curPts.size() < 4) {
234  for (unsigned int i = 0; i < 3; i += 1) {
235  for (unsigned int j = 0; j < 3; j += 1) {
236  if (i == j) {
237  homography[i][j] = 1;
238  } else {
239  homography[i][j] = 0;
240  }
241  }
242  }
243  return false;
244  }
245 
246  /* part of code from OpenCV planarObjectDetector */
247  std::vector<unsigned char> mask;
248  H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
249 
250  if (H.data) {
251  const cv::Mat_<double> &H_tmp = H;
252  dst_corners.resize(4);
253  for (unsigned int i = 0; i < 4; i++) {
254  cv::Point2f pt = ref_corners[i];
255 
256  double w = 1. / (H_tmp(2, 0) * pt.x + H_tmp(2, 1) * pt.y + H_tmp(2, 2));
257  dst_corners[i] = cv::Point2f((float)((H_tmp(0, 0) * pt.x + H_tmp(0, 1) * pt.y + H_tmp(0, 2)) * w),
258  (float)((H_tmp(1, 0) * pt.x + H_tmp(1, 1) * pt.y + H_tmp(1, 2)) * w));
259  }
260 
261  double *ptr = (double *)H_tmp.data;
262  for (unsigned int i = 0; i < 9; i++) {
263  this->homography[(unsigned int)(i / 3)][i % 3] = *(ptr++);
264  }
265  isCorrect = true;
266  } else {
267  isCorrect = false;
268  }
269 
270  currentImagePoints.resize(0);
271  refImagePoints.resize(0);
272  for (unsigned int i = 0; i < mask.size(); i += 1) {
273  if (mask[i] != 0) {
274  vpImagePoint ip;
275  ip.set_i(curPts[i].y);
276  ip.set_j(curPts[i].x);
277  currentImagePoints.push_back(ip);
278  ip.set_i(refPts[i].y);
279  ip.set_j(refPts[i].x);
280  refImagePoints.push_back(ip);
281  }
282  }
283 
284  if (currentImagePoints.size() < minNbMatching) {
285  isCorrect = false;
286  }
287 
288  return isCorrect;
289 }
290 
304 bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
305  const unsigned int height, const unsigned int width)
306 {
307  if ((iP.get_i() + height) >= I.getHeight() || (iP.get_j() + width) >= I.getWidth()) {
308  vpTRACE("Bad size for the subimage");
309  throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage"));
310  }
311 
312  vpImage<unsigned char> subImage;
313 
314  vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), height, width, subImage);
315 
316  return this->matchPoint(subImage);
317 }
318 
330 bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle)
331 {
332  vpImagePoint iP;
333  iP.set_i(rectangle.getTop());
334  iP.set_j(rectangle.getLeft());
335  return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
336 }
337 
348 void vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
349 {
350  for (unsigned int i = 0; i < dst_corners.size(); i++) {
351  vpImagePoint ip1(dst_corners[i].y - modelROI.y, dst_corners[i].x - modelROI.x);
352  vpImagePoint ip2(dst_corners[(i + 1) % dst_corners.size()].y - modelROI.y,
353  dst_corners[(i + 1) % dst_corners.size()].x - modelROI.x);
354  vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
355  }
356 
357  if (displayKpts) {
358  for (unsigned int i = 0; i < currentImagePoints.size(); ++i) {
359  vpImagePoint ip(currentImagePoints[i].get_i() - modelROI.y, currentImagePoints[i].get_j() - modelROI.x);
361  }
362  }
363 }
364 
382 void vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref, vpImage<unsigned char> &Icurrent, bool displayKpts)
383 {
384  display(Icurrent, displayKpts);
385 
386  if (displayKpts) {
387  for (unsigned int i = 0; i < refImagePoints.size(); ++i) {
388  vpDisplay::displayCross(Iref, refImagePoints[i], 5, vpColor::green);
389  }
390  }
391 }
392 
403 void vpPlanarObjectDetector::load(const std::string &dataFilename, const std::string &objName)
404 {
405  fern.load(dataFilename, objName);
406  modelROI = fern.getModelROI();
407  initialiseRefCorners(modelROI);
408 }
409 
417 void vpPlanarObjectDetector::recordDetector(const std::string &objectName, const std::string &dataFile)
418 {
419  fern.record(objectName, dataFile);
420 }
421 
427 std::vector<vpImagePoint> vpPlanarObjectDetector::getDetectedCorners() const
428 {
429  vpImagePoint ip;
430  std::vector<vpImagePoint> corners;
431  corners.clear();
432  for (unsigned int i = 0; i < dst_corners.size(); i++) {
433  ip.set_uv(dst_corners[i].x, dst_corners[i].y);
434  corners.push_back(ip);
435  }
436 
437  return corners;
438 }
439 
445 void vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect &_modelROI)
446 {
447  cv::Point2f ip;
448 
449  ip.y = (float)_modelROI.y;
450  ip.x = (float)_modelROI.x;
451  ref_corners.push_back(ip);
452 
453  ip.y = (float)(_modelROI.y + _modelROI.height);
454  ip.x = (float)_modelROI.x;
455  ref_corners.push_back(ip);
456 
457  ip.y = (float)(_modelROI.y + _modelROI.height);
458  ip.x = (float)(_modelROI.x + _modelROI.width);
459  ref_corners.push_back(ip);
460 
461  ip.y = (float)_modelROI.y;
462  ip.x = (float)(_modelROI.x + _modelROI.width);
463  ref_corners.push_back(ip);
464 }
465 
466 void vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint &_imPoint)
467 {
468  if (_i >= refImagePoints.size()) {
469  throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
470  }
471  _imPoint = refImagePoints[_i];
472 }
473 
474 void vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint &_referencePoint,
475  vpImagePoint &_currentPoint)
476 {
477  // fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
478  if (_index >= currentImagePoints.size()) {
479  throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
480  }
481 
482  _referencePoint = refImagePoints[_index];
483  _currentPoint = currentImagePoints[_index];
484 }
485 
486 #elif !defined(VISP_BUILD_SHARED_LIBS)
487 // Work arround to avoid warning:
488 // libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
489 void dummy_vpPlanarObjectDetector(){};
490 #endif
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
double getTop() const
Definition: vpRect.h:175
double get_v() const
Definition: vpImagePoint.h:274
double get_i() const
Definition: vpImagePoint.h:204
unsigned int getWidth() const
Definition: vpImage.h:239
double get_u() const
Definition: vpImagePoint.h:263
error that can be emited by ViSP classes.
Definition: vpException.h:71
double getHeight() const
Definition: vpRect.h:149
static const vpColor green
Definition: vpColor.h:183
double get_j() const
Definition: vpImagePoint.h:215
vpImagePoint getTopLeft() const
Definition: vpRect.h:182
static const vpColor red
Definition: vpColor.h:180
void set_i(const double ii)
Definition: vpImagePoint.h:167
double getWidth() const
Definition: vpRect.h:197
#define vpTRACE
Definition: vpDebug.h:416
void set_j(const double jj)
Definition: vpImagePoint.h:178
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:272
unsigned int getHeight() const
Definition: vpImage.h:178
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:248
Defines a rectangle in the plane.
Definition: vpRect.h:78
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
double getLeft() const
Definition: vpRect.h:156