Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPlanarObjectDetector.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Planar surface detection tool.
32  *
33  * Authors:
34  * Romain Tallonneau
35  *
36  *****************************************************************************/
37 
38 #include <visp3/vision/vpPlanarObjectDetector.h>
39 
40 #if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
41 
42 #include <visp3/core/vpImageConvert.h>
43 #include <visp3/core/vpException.h>
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpDisplay.h>
46 #include <visp3/core/vpColor.h>
47 #include <visp3/core/vpImageTools.h>
48 
49 #include <vector>
50 #include <iostream>
51 
58  : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
59  refImagePoints(), minNbMatching(10)
60 {
61 }
62 
71 vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, const std::string& _objectName)
72  : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
73  refImagePoints(), minNbMatching(10)
74 {
75  load(_dataFile, _objectName);
76 }
77 
81 void
83 {
84 }
85 
92 {
93 
94 }
95 
103 void
105 {
106  if(nbpt < 3){
107  throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
108  }
109 
110  std::vector < vpImagePoint > ptsx(nbpt);
111  std::vector < vpImagePoint > ptsy(nbpt);
112  for(unsigned int i=0; i<nbpt; i++){
113  ptsx[i] = ptsy[i] = ip[i];
114  }
115 
116  for(unsigned int i=0; i<nbpt; i++){
117  for(unsigned int j=0; j<nbpt-1; j++){
118  if(ptsx[j].get_j() > ptsx[j+1].get_j()){
119  double tmp = ptsx[j+1].get_j();
120  ptsx[j+1].set_j(ptsx[j].get_j());
121  ptsx[j].set_j(tmp);
122  }
123  }
124  }
125  for(unsigned int i=0; i<nbpt; i++){
126  for(unsigned int j=0; j<nbpt-1; j++){
127  if(ptsy[j].get_i() > ptsy[j+1].get_i()){
128  double tmp = ptsy[j+1].get_i();
129  ptsy[j+1].set_i(ptsy[j].get_i());
130  ptsy[j].set_i(tmp);
131  }
132  }
133  }
134 
135 }
136 
137 
148 unsigned int
150 {
151  modelROI.x = 0;
152  modelROI.y = 0;
153  modelROI.width = (int)_I.getWidth();
154  modelROI.height = (int)_I.getHeight();
155 
157 
158  return fern.buildReference(_I);
159 }
160 
161 
175 unsigned int
177  const vpImagePoint &_iP,
178  unsigned int _height, unsigned int _width)
179 {
180  unsigned int res = fern.buildReference(_I, _iP, _height, _width);
181  modelROI.x = (int)_iP.get_u();
182  modelROI.y = (int)_iP.get_v();
183  modelROI.width = (int)_width;
184  modelROI.height = (int)_height;
185 
187 
188  return res;
189 }
190 
191 
192 
203 unsigned int
205  const vpRect _rectangle)
206 {
207  unsigned int res = fern.buildReference(_I, _rectangle);
208 
209  vpImagePoint iP = _rectangle.getTopLeft();
210 
211  modelROI.x = (int)iP.get_u();
212  modelROI.y = (int)iP.get_v();
213  modelROI.width = (int)_rectangle.getWidth();
214  modelROI.height = (int)_rectangle.getHeight();
215 
217 
218  return res;
219 }
220 
221 
232 bool
234 {
235  fern.matchPoint(I);
236 
237  /* compute homography */
238  std::vector<cv::Point2f> refPts = fern.getRefPt();
239  std::vector<cv::Point2f> curPts = fern.getCurPt();
240 
241  for(unsigned int i=0; i<refPts.size(); ++i){
242  refPts[i].x += modelROI.x;
243  refPts[i].y += modelROI.y;
244  }
245  for(unsigned int i=0; i<curPts.size(); ++i){
246  curPts[i].x += modelROI.x;
247  curPts[i].y += modelROI.y;
248  }
249 
250  if(curPts.size() < 4){
251  for (unsigned int i = 0; i < 3; i += 1){
252  for (unsigned int j = 0; j < 3; j += 1){
253  if(i == j){
254  homography[i][j] = 1;
255  }
256  else{
257  homography[i][j] = 0;
258  }
259  }
260  }
261  return false;
262  }
263 
264  /* part of code from OpenCV planarObjectDetector */
265  std::vector<unsigned char> mask;
266  H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
267 
268  if( H.data )
269  {
270  const cv::Mat_<double>& H_tmp = H;
271  dst_corners.resize(4);
272  for(unsigned int i = 0; i < 4; i++ )
273  {
274  cv::Point2f pt = ref_corners[i];
275 
276  double w = 1./(H_tmp(2,0)*pt.x + H_tmp(2,1)*pt.y + H_tmp(2,2));
277  dst_corners[i] = cv::Point2f((float)((H_tmp(0,0)*pt.x + H_tmp(0,1)*pt.y + H_tmp(0,2))*w),
278  (float)((H_tmp(1,0)*pt.x + H_tmp(1,1)*pt.y + H_tmp(1,2))*w));
279  }
280 
281  double* ptr = (double*)H_tmp.data;
282  for(unsigned int i=0; i<9; i++){
283  this->homography[(unsigned int)(i/3)][i%3] = *(ptr++);
284  }
285  isCorrect = true;
286  }
287  else{
288  isCorrect = false;
289  }
290 
291 
292  currentImagePoints.resize(0);
293  refImagePoints.resize(0);
294  for (unsigned int i = 0; i < mask.size(); i += 1){
295  if(mask[i] != 0){
296  vpImagePoint ip;
297  ip.set_i(curPts[i].y);
298  ip.set_j(curPts[i].x);
299  currentImagePoints.push_back(ip);
300  ip.set_i(refPts[i].y);
301  ip.set_j(refPts[i].x);
302  refImagePoints.push_back(ip);
303  }
304  }
305 
306  if(currentImagePoints.size() < minNbMatching){
307  isCorrect = false;
308  }
309 
310  return isCorrect;
311 }
312 
313 
314 
328 bool
330  const vpImagePoint &iP, const unsigned int height, const unsigned int width)
331 {
332  if((iP.get_i()+height) >= I.getHeight()
333  || (iP.get_j()+width) >= I.getWidth()) {
334  vpTRACE("Bad size for the subimage");
336  "Bad size for the subimage"));
337  }
338 
339  vpImage<unsigned char> subImage;
340 
342  (unsigned int)iP.get_i(),
343  (unsigned int)iP.get_j(),
344  height, width, subImage);
345 
346  return this->matchPoint(subImage);
347 }
348 
360 bool
362 {
363  vpImagePoint iP;
364  iP.set_i(rectangle.getTop());
365  iP.set_j(rectangle.getLeft());
366  return (this->matchPoint(I, iP,
367  (unsigned int)rectangle.getHeight(),
368  (unsigned int)rectangle.getWidth()));
369 }
370 
371 
381 void
383 {
384  for(unsigned int i=0; i<dst_corners.size(); i++){
385  vpImagePoint ip1(
386  dst_corners[i].y - modelROI.y,
387  dst_corners[i].x - modelROI.x);
388  vpImagePoint ip2(
389  dst_corners[(i+1)%dst_corners.size()].y - modelROI.y,
390  dst_corners[(i+1)%dst_corners.size()].x - modelROI.x);
391  vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
392  }
393 
394  if(displayKpts){
395  for(unsigned int i=0; i<currentImagePoints.size(); ++i){
396  vpImagePoint ip(
397  currentImagePoints[i].get_i() - modelROI.y,
398  currentImagePoints[i].get_j() - modelROI.x);
400  }
401  }
402 }
403 
404 
421 void
423  vpImage<unsigned char> &Icurrent, bool displayKpts)
424 {
425  display(Icurrent, displayKpts);
426 
427  if(displayKpts){
428  for(unsigned int i=0; i<refImagePoints.size(); ++i){
430  }
431  }
432 }
433 
434 
445 void
446 vpPlanarObjectDetector::load(const std::string& dataFilename, const std::string& objName)
447 {
448  fern.load(dataFilename, objName);
451 }
452 
453 
461 void
462 vpPlanarObjectDetector::recordDetector(const std::string& objectName, const std::string& dataFile )
463 {
464  fern.record(objectName, dataFile);
465 }
466 
467 
468 
474 std::vector<vpImagePoint>
476  vpImagePoint ip;
477  std::vector <vpImagePoint> corners;
478  corners.clear();
479  for(unsigned int i=0; i<dst_corners.size(); i++){
480  ip.set_uv( dst_corners[i].x, dst_corners[i].y);
481  corners.push_back(ip);
482  }
483 
484  return corners;
485 }
486 
492 void
494 {
495  cv::Point2f ip;
496 
497  ip.y = (float)_modelROI.y;
498  ip.x = (float)_modelROI.x;
499  ref_corners.push_back(ip);
500 
501  ip.y = (float)(_modelROI.y+_modelROI.height);
502  ip.x = (float)_modelROI.x;
503  ref_corners.push_back(ip);
504 
505  ip.y = (float)(_modelROI.y+_modelROI.height);
506  ip.x = (float)(_modelROI.x+_modelROI.width);
507  ref_corners.push_back(ip);
508 
509  ip.y = (float)_modelROI.y;
510  ip.x = (float)(_modelROI.x+_modelROI.width);
511  ref_corners.push_back(ip);
512 }
513 
514 
515 void
517 {
518  if(_i >= refImagePoints.size()){
519  throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
520  }
521  _imPoint = refImagePoints[_i];
522 }
523 
524 void
525 vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint)
526 {
527 // fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
528  if(_index >= currentImagePoints.size()){
529  throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
530  }
531 
532  _referencePoint = refImagePoints[_index];
533  _currentPoint = currentImagePoints[_index];
534 }
535 
536 #elif !defined(VISP_BUILD_SHARED_LIBS)
537 // Work arround to avoid warning: libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
538 void dummy_vpPlanarObjectDetector() {};
539 #endif
540 
void initialiseRefCorners(const cv::Rect &_modelROI)
double getTop() const
Definition: vpRect.h:178
double get_v() const
Definition: vpImagePoint.h:268
double get_i() const
Definition: vpImagePoint.h:199
unsigned int getWidth() const
Definition: vpImage.h:226
virtual unsigned int matchPoint(const vpImage< unsigned char > &I)
void computeRoi(vpImagePoint *ip, const unsigned int nbpt)
void getMatchedPoints(const unsigned int _index, vpImagePoint &_referencePoint, vpImagePoint &_currentPoint)
double get_u() const
Definition: vpImagePoint.h:257
std::vector< cv::Point2f > ref_corners
The corners in the reference image.
void load(const std::string &dataFilename, const std::string &objName)
Load the Fern classifier.
error that can be emited by ViSP classes.
Definition: vpException.h:73
void getReferencePoint(const unsigned int _i, vpImagePoint &_imPoint)
double getHeight() const
Definition: vpRect.h:152
cv::Rect modelROI
The ROI for the reference image.
const std::vector< cv::Point2f > & getRefPt() const
static const vpColor green
Definition: vpColor.h:166
double get_j() const
Definition: vpImagePoint.h:210
vpImagePoint getTopLeft() const
Definition: vpRect.h:185
cv::Mat H
Computed homography in the OpenCV format.
static const vpColor red
Definition: vpColor.h:163
bool matchPoint(const vpImage< unsigned char > &I)
virtual unsigned int buildReference(const vpImage< unsigned char > &I)
std::vector< cv::Point2f > dst_corners
The estimated new coordinates of the corners (reprojected using the homography).
void set_i(const double ii)
Definition: vpImagePoint.h:163
void display(vpImage< unsigned char > &I, bool displayKpts=false)
double getWidth() const
Definition: vpRect.h:199
vpFernClassifier fern
Fern Classifier used to match the points between a reference image and the current image...
std::vector< vpImagePoint > currentImagePoints
Vector of the image point in the current image that match after the deletion of the outliers with the...
#define vpTRACE
Definition: vpDebug.h:414
vpHomography homography
Computed homography in the ViSP format.
unsigned int buildReference(const vpImage< unsigned char > &I)
const std::vector< cv::Point2f > & getCurPt() const
void record(const std::string &_objectName, const std::string &_dataFile)
record the Ferns classifier in the text file
bool isCorrect
Flag to indicate wether the last computed homography is correct or not.
void set_j(const double jj)
Definition: vpImagePoint.h:174
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
cv::Rect getModelROI() const
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:266
void recordDetector(const std::string &objectName, const std::string &dataFile)
Record the Ferns classifier in the text file.
unsigned int minNbMatching
Minimal number of point to after the ransac needed to suppose that the homography has been correctly ...
unsigned int getHeight() const
Definition: vpImage.h:175
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:243
Defines a rectangle in the plane.
Definition: vpRect.h:82
std::vector< vpImagePoint > getDetectedCorners() const
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)
void load(const std::string &_dataFile, const std::string &)
load the Fern classifier
std::vector< vpImagePoint > refImagePoints
Vector of the image point in the reference image that match after the deletion of the outliers with t...
double getLeft() const
Definition: vpRect.h:159