ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpPlanarObjectDetector.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPlanarObjectDetector.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Planar surface detection tool.
36  *
37  * Authors:
38  * Romain Tallonneau
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpPlanarObjectDetector.h>
43 
44 #if (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
45 
46 #include <visp/vpImageConvert.h>
47 #include <visp/vpException.h>
48 #include <visp/vpImagePoint.h>
49 #include <visp/vpDisplay.h>
50 #include <visp/vpDisplayX.h>
51 #include <visp/vpColor.h>
52 #include <visp/vpImageTools.h>
53 
54 #include <vector>
55 #include <iostream>
56 
63 {
64  isCorrect = false;
65  dst_corners.resize(0);
66  ref_corners.resize(0);
67  currentImagePoints.resize(0);
68  refImagePoints.resize(0);
69  minNbMatching = 10;
70 }
71 
80 vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, const std::string& _objectName)
81 {
82  isCorrect = false;
83  load(_dataFile, _objectName);
84 }
85 
90 void
92 {
93 
94 }
95 
102 {
103 
104 }
105 
113 void
115 {
116  if(nbpt < 3){
117  throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
118  }
119 
120  std::vector < vpImagePoint > ptsx(nbpt);
121  std::vector < vpImagePoint > ptsy(nbpt);
122  for(unsigned int i=0; i<nbpt; i++){
123  ptsx[i] = ptsy[i] = ip[i];
124  }
125 
126  for(unsigned int i=0; i<nbpt; i++){
127  for(unsigned int j=0; j<nbpt-1; j++){
128  if(ptsx[j].get_j() > ptsx[j+1].get_j()){
129  double tmp = ptsx[j+1].get_j();
130  ptsx[j+1].set_j(ptsx[j].get_j());
131  ptsx[j].set_j(tmp);
132  }
133  }
134  }
135  for(unsigned int i=0; i<nbpt; i++){
136  for(unsigned int j=0; j<nbpt-1; j++){
137  if(ptsy[j].get_i() > ptsy[j+1].get_i()){
138  double tmp = ptsy[j+1].get_i();
139  ptsy[j+1].set_i(ptsy[j].get_i());
140  ptsy[j].set_i(tmp);
141  }
142  }
143  }
144 
145 }
146 
147 
158 unsigned int
160 {
161  modelROI.x = 0;
162  modelROI.y = 0;
163  modelROI.width = (int)_I.getWidth();
164  modelROI.height = (int)_I.getHeight();
165 
167 
168  return fern.buildReference(_I);
169 }
170 
171 
185 unsigned int
187  const vpImagePoint &_iP,
188  unsigned int _height, unsigned int _width)
189 {
190  unsigned int res = fern.buildReference(_I, _iP, _height, _width);
191  modelROI.x = (int)_iP.get_u();
192  modelROI.y = (int)_iP.get_v();
193  modelROI.width = (int)_width;
194  modelROI.height = (int)_height;
195 
197 
198  return res;
199 }
200 
201 
202 
213 unsigned int
215  const vpRect _rectangle)
216 {
217  unsigned int res = fern.buildReference(_I, _rectangle);
218 
219  vpImagePoint iP = _rectangle.getTopLeft();
220 
221  modelROI.x = (int)iP.get_u();
222  modelROI.y = (int)iP.get_v();
223  modelROI.width = (int)_rectangle.getWidth();
224  modelROI.height = (int)_rectangle.getHeight();
225 
227 
228  return res;
229 }
230 
231 
242 bool
244 {
245  fern.matchPoint(I);
246 
247  /* compute homography */
248  std::vector<cv::Point2f> refPts = fern.getRefPt();
249  std::vector<cv::Point2f> curPts = fern.getCurPt();
250 
251  for(unsigned int i=0; i<refPts.size(); ++i){
252  refPts[i].x += modelROI.x;
253  refPts[i].y += modelROI.y;
254  }
255  for(unsigned int i=0; i<curPts.size(); ++i){
256  curPts[i].x += modelROI.x;
257  curPts[i].y += modelROI.y;
258  }
259 
260  if(curPts.size() < 4){
261  for (unsigned int i = 0; i < 3; i += 1){
262  for (unsigned int j = 0; j < 3; j += 1){
263  if(i == j){
264  homography[i][j] = 1;
265  }
266  else{
267  homography[i][j] = 0;
268  }
269  }
270  }
271  return false;
272  }
273 
274  /* part of code from OpenCV planarObjectDetector */
275  std::vector<unsigned char> mask;
276  H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
277 
278  if( H.data )
279  {
280  const cv::Mat_<double>& H_tmp = H;
281  dst_corners.resize(4);
282  for(unsigned int i = 0; i < 4; i++ )
283  {
284  cv::Point2f pt = ref_corners[i];
285 
286  double w = 1./(H_tmp(2,0)*pt.x + H_tmp(2,1)*pt.y + H_tmp(2,2));
287  dst_corners[i] = cv::Point2f((float)((H_tmp(0,0)*pt.x + H_tmp(0,1)*pt.y + H_tmp(0,2))*w),
288  (float)((H_tmp(1,0)*pt.x + H_tmp(1,1)*pt.y + H_tmp(1,2))*w));
289  }
290 
291  double* ptr = (double*)H_tmp.data;
292  for(unsigned int i=0; i<9; i++){
293  this->homography[(unsigned int)(i/3)][i%3] = *(ptr++);
294  }
295  isCorrect = true;
296  }
297  else{
298  isCorrect = false;
299  }
300 
301 
302  currentImagePoints.resize(0);
303  refImagePoints.resize(0);
304  for (unsigned int i = 0; i < mask.size(); i += 1){
305  if(mask[i] != 0){
306  vpImagePoint ip;
307  ip.set_i(curPts[i].y);
308  ip.set_j(curPts[i].x);
309  currentImagePoints.push_back(ip);
310  ip.set_i(refPts[i].y);
311  ip.set_j(refPts[i].x);
312  refImagePoints.push_back(ip);
313  }
314  }
315 
316  if(currentImagePoints.size() < minNbMatching){
317  isCorrect = false;
318  }
319 
320  return isCorrect;
321 }
322 
323 
324 
338 bool
340  const vpImagePoint &iP, const unsigned int height, const unsigned int width)
341 {
342  if((iP.get_i()+height) >= I.getHeight()
343  || (iP.get_j()+width) >= I.getWidth()) {
344  vpTRACE("Bad size for the subimage");
346  "Bad size for the subimage"));
347  }
348 
349  vpImage<unsigned char> subImage;
350 
352  (unsigned int)iP.get_i(),
353  (unsigned int)iP.get_j(),
354  height, width, subImage);
355 
356  return this->matchPoint(subImage);
357 }
358 
370 bool
372 {
373  vpImagePoint iP;
374  iP.set_i(rectangle.getTop());
375  iP.set_j(rectangle.getLeft());
376  return (this->matchPoint(I, iP,
377  (unsigned int)rectangle.getHeight(),
378  (unsigned int)rectangle.getWidth()));
379 }
380 
381 
391 void
393 {
394  for(unsigned int i=0; i<dst_corners.size(); i++){
395  vpImagePoint ip1(
396  dst_corners[i].y - modelROI.y,
397  dst_corners[i].x - modelROI.x);
398  vpImagePoint ip2(
399  dst_corners[(i+1)%dst_corners.size()].y - modelROI.y,
400  dst_corners[(i+1)%dst_corners.size()].x - modelROI.x);
401  vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
402  }
403 
404  if(displayKpts){
405  for(unsigned int i=0; i<currentImagePoints.size(); ++i){
406  vpImagePoint ip(
407  currentImagePoints[i].get_i() - modelROI.y,
408  currentImagePoints[i].get_j() - modelROI.x);
410  }
411  }
412 }
413 
414 
431 void
433  vpImage<unsigned char> &Icurrent, bool displayKpts)
434 {
435  display(Icurrent, displayKpts);
436 
437  if(displayKpts){
438  for(unsigned int i=0; i<refImagePoints.size(); ++i){
440  }
441  }
442 }
443 
444 
455 void
456 vpPlanarObjectDetector::load(const std::string& dataFilename, const std::string& objName)
457 {
458  fern.load(dataFilename, objName);
461 }
462 
463 
471 void
472 vpPlanarObjectDetector::recordDetector(const std::string& objectName, const std::string& dataFile )
473 {
474  fern.record(objectName, dataFile);
475 }
476 
477 
478 
484 std::vector<vpImagePoint>
486  vpImagePoint ip;
487  std::vector <vpImagePoint> corners;
488  corners.clear();
489  for(unsigned int i=0; i<dst_corners.size(); i++){
490  ip.set_uv( dst_corners[i].x, dst_corners[i].y);
491  corners.push_back(ip);
492  }
493 
494  return corners;
495 }
496 
502 void
504 {
505  cv::Point2f ip;
506 
507  ip.y = (float)_modelROI.y;
508  ip.x = (float)_modelROI.x;
509  ref_corners.push_back(ip);
510 
511  ip.y = (float)(_modelROI.y+_modelROI.height);
512  ip.x = (float)_modelROI.x;
513  ref_corners.push_back(ip);
514 
515  ip.y = (float)(_modelROI.y+_modelROI.height);
516  ip.x = (float)(_modelROI.x+_modelROI.width);
517  ref_corners.push_back(ip);
518 
519  ip.y = (float)_modelROI.y;
520  ip.x = (float)(_modelROI.x+_modelROI.width);
521  ref_corners.push_back(ip);
522 }
523 
524 
525 void
527 {
528  if(_i >= refImagePoints.size()){
529  throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
530  }
531  _imPoint = refImagePoints[_i];
532 }
533 
534 void
535 vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint)
536 {
537 // fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
538  if(_index >= currentImagePoints.size()){
539  throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
540  }
541 
542  _referencePoint = refImagePoints[_index];
543  _currentPoint = currentImagePoints[_index];
544 }
545 
546 #endif
547