Visual Servoing Platform version 3.5.0
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
59vpPlanarObjectDetector::vpPlanarObjectDetector()
60 : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
61 refImagePoints(), minNbMatching(10)
62{
63}
64
73vpPlanarObjectDetector::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
83void vpPlanarObjectDetector::init() {}
84
90vpPlanarObjectDetector::~vpPlanarObjectDetector() {}
91
99void vpPlanarObjectDetector::computeRoi(vpImagePoint *ip, 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
141unsigned 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
166unsigned 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
190unsigned 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
216bool 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
304bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
305 unsigned int height, 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
330bool 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
348void 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);
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
382void 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
403void 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
417void vpPlanarObjectDetector::recordDetector(const std::string &objectName, const std::string &dataFile)
418{
419 fern.record(objectName, dataFile);
420}
421
427std::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
445void 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
466void 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
474void 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
489void dummy_vpPlanarObjectDetector(){};
490#endif
static const vpColor red
Definition: vpColor.h:217
static const vpColor green
Definition: vpColor.h:220
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ fatalError
Fatal error.
Definition: vpException.h:96
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
double get_j() const
Definition: vpImagePoint.h:214
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_u() const
Definition: vpImagePoint.h:262
void set_uv(double u, double v)
Definition: vpImagePoint.h:247
double get_i() const
Definition: vpImagePoint.h:203
double get_v() const
Definition: vpImagePoint.h:273
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:305
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getWidth() const
Definition: vpRect.h:228
double getLeft() const
Definition: vpRect.h:174
vpImagePoint getTopLeft() const
Definition: vpRect.h:200
double getHeight() const
Definition: vpRect.h:167
double getTop() const
Definition: vpRect.h:193
#define vpTRACE
Definition: vpDebug.h:416