Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPlanarObjectDetector.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
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 https://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
34#include <visp3/vision/vpPlanarObjectDetector.h>
35
36#if (VISP_HAVE_OPENCV_VERSION >= 0x020408) && \
37 (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.4.8 and < 3.0.0
38
39#include <visp3/core/vpColor.h>
40#include <visp3/core/vpDisplay.h>
41#include <visp3/core/vpException.h>
42#include <visp3/core/vpImageConvert.h>
43#include <visp3/core/vpImagePoint.h>
44#include <visp3/core/vpImageTools.h>
45
46#include <iostream>
47#include <vector>
48
54vpPlanarObjectDetector::vpPlanarObjectDetector()
55 : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
56 refImagePoints(), minNbMatching(10)
57{ }
58
67vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string &_dataFile, const std::string &_objectName)
68 : fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
69 refImagePoints(), minNbMatching(10)
70{
71 load(_dataFile, _objectName);
72}
73
77void vpPlanarObjectDetector::init() { }
78
84vpPlanarObjectDetector::~vpPlanarObjectDetector() { }
85
93void vpPlanarObjectDetector::computeRoi(vpImagePoint *ip, unsigned int nbpt)
94{
95 if (nbpt < 3) {
96 throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
97 }
98
99 std::vector<vpImagePoint> ptsx(nbpt);
100 std::vector<vpImagePoint> ptsy(nbpt);
101 for (unsigned int i = 0; i < nbpt; i++) {
102 ptsx[i] = ptsy[i] = ip[i];
103 }
104
105 for (unsigned int i = 0; i < nbpt; i++) {
106 for (unsigned int j = 0; j < nbpt - 1; j++) {
107 if (ptsx[j].get_j() > ptsx[j + 1].get_j()) {
108 double tmp = ptsx[j + 1].get_j();
109 ptsx[j + 1].set_j(ptsx[j].get_j());
110 ptsx[j].set_j(tmp);
111 }
112 }
113 }
114 for (unsigned int i = 0; i < nbpt; i++) {
115 for (unsigned int j = 0; j < nbpt - 1; j++) {
116 if (ptsy[j].get_i() > ptsy[j + 1].get_i()) {
117 double tmp = ptsy[j + 1].get_i();
118 ptsy[j + 1].set_i(ptsy[j].get_i());
119 ptsy[j].set_i(tmp);
120 }
121 }
122 }
123}
124
135unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
136{
137 modelROI.x = 0;
138 modelROI.y = 0;
139 modelROI.width = (int)_I.getWidth();
140 modelROI.height = (int)_I.getHeight();
141
142 initialiseRefCorners(modelROI);
143
144 return fern.buildReference(_I);
145}
146
160unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I, const vpImagePoint &_iP,
161 unsigned int _height, unsigned int _width)
162{
163 unsigned int res = fern.buildReference(_I, _iP, _height, _width);
164 modelROI.x = (int)_iP.get_u();
165 modelROI.y = (int)_iP.get_v();
166 modelROI.width = (int)_width;
167 modelROI.height = (int)_height;
168
169 initialiseRefCorners(modelROI);
170
171 return res;
172}
173
184unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I, const vpRect &_rectangle)
185{
186 unsigned int res = fern.buildReference(_I, _rectangle);
187
188 vpImagePoint iP = _rectangle.getTopLeft();
189
190 modelROI.x = (int)iP.get_u();
191 modelROI.y = (int)iP.get_v();
192 modelROI.width = (int)_rectangle.getWidth();
193 modelROI.height = (int)_rectangle.getHeight();
194
195 initialiseRefCorners(modelROI);
196
197 return res;
198}
199
210bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
211{
212 fern.matchPoint(I);
213
214 /* compute homography */
215 std::vector<cv::Point2f> refPts = fern.getRefPt();
216 std::vector<cv::Point2f> curPts = fern.getCurPt();
217
218 for (unsigned int i = 0; i < refPts.size(); ++i) {
219 refPts[i].x += modelROI.x;
220 refPts[i].y += modelROI.y;
221 }
222 for (unsigned int i = 0; i < curPts.size(); ++i) {
223 curPts[i].x += modelROI.x;
224 curPts[i].y += modelROI.y;
225 }
226
227 if (curPts.size() < 4) {
228 for (unsigned int i = 0; i < 3; i += 1) {
229 for (unsigned int j = 0; j < 3; j += 1) {
230 if (i == j) {
231 homography[i][j] = 1;
232 }
233 else {
234 homography[i][j] = 0;
235 }
236 }
237 }
238 return false;
239 }
240
241 /* part of code from OpenCV planarObjectDetector */
242 std::vector<unsigned char> mask;
243 H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
244
245 if (H.data) {
246 const cv::Mat_<double> &H_tmp = H;
247 dst_corners.resize(4);
248 for (unsigned int i = 0; i < 4; i++) {
249 cv::Point2f pt = ref_corners[i];
250
251 double w = 1. / (H_tmp(2, 0) * pt.x + H_tmp(2, 1) * pt.y + H_tmp(2, 2));
252 dst_corners[i] = cv::Point2f((float)((H_tmp(0, 0) * pt.x + H_tmp(0, 1) * pt.y + H_tmp(0, 2)) * w),
253 (float)((H_tmp(1, 0) * pt.x + H_tmp(1, 1) * pt.y + H_tmp(1, 2)) * w));
254 }
255
256 double *ptr = (double *)H_tmp.data;
257 for (unsigned int i = 0; i < 9; i++) {
258 this->homography[(unsigned int)(i / 3)][i % 3] = *(ptr++);
259 }
260 isCorrect = true;
261 }
262 else {
263 isCorrect = false;
264 }
265
266 currentImagePoints.resize(0);
267 refImagePoints.resize(0);
268 for (unsigned int i = 0; i < mask.size(); i += 1) {
269 if (mask[i] != 0) {
270 vpImagePoint ip;
271 ip.set_i(curPts[i].y);
272 ip.set_j(curPts[i].x);
273 currentImagePoints.push_back(ip);
274 ip.set_i(refPts[i].y);
275 ip.set_j(refPts[i].x);
276 refImagePoints.push_back(ip);
277 }
278 }
279
280 if (currentImagePoints.size() < minNbMatching) {
281 isCorrect = false;
282 }
283
284 return isCorrect;
285}
286
300bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, unsigned int height,
301 unsigned int width)
302{
303 if ((iP.get_i() + height) >= I.getHeight() || (iP.get_j() + width) >= I.getWidth()) {
304 vpTRACE("Bad size for the subimage");
305 throw(vpException(vpImageException::notInTheImage, "Bad size for the subimage"));
306 }
307
308 vpImage<unsigned char> subImage;
309
310 vpImageTools::crop(I, (unsigned int)iP.get_i(), (unsigned int)iP.get_j(), height, width, subImage);
311
312 return this->matchPoint(subImage);
313}
314
326bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle)
327{
328 vpImagePoint iP;
329 iP.set_i(rectangle.getTop());
330 iP.set_j(rectangle.getLeft());
331 return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
332}
333
344void vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
345{
346 for (unsigned int i = 0; i < dst_corners.size(); i++) {
347 vpImagePoint ip1(dst_corners[i].y - modelROI.y, dst_corners[i].x - modelROI.x);
348 vpImagePoint ip2(dst_corners[(i + 1) % dst_corners.size()].y - modelROI.y,
349 dst_corners[(i + 1) % dst_corners.size()].x - modelROI.x);
351 }
352
353 if (displayKpts) {
354 for (unsigned int i = 0; i < currentImagePoints.size(); ++i) {
355 vpImagePoint ip(currentImagePoints[i].get_i() - modelROI.y, currentImagePoints[i].get_j() - modelROI.x);
357 }
358 }
359}
360
378void vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref, vpImage<unsigned char> &Icurrent, bool displayKpts)
379{
380 display(Icurrent, displayKpts);
381
382 if (displayKpts) {
383 for (unsigned int i = 0; i < refImagePoints.size(); ++i) {
384 vpDisplay::displayCross(Iref, refImagePoints[i], 5, vpColor::green);
385 }
386 }
387}
388
399void vpPlanarObjectDetector::load(const std::string &dataFilename, const std::string &objName)
400{
401 fern.load(dataFilename, objName);
402 modelROI = fern.getModelROI();
403 initialiseRefCorners(modelROI);
404}
405
413void vpPlanarObjectDetector::recordDetector(const std::string &objectName, const std::string &dataFile)
414{
415 fern.record(objectName, dataFile);
416}
417
423std::vector<vpImagePoint> vpPlanarObjectDetector::getDetectedCorners() const
424{
425 vpImagePoint ip;
426 std::vector<vpImagePoint> corners;
427 corners.clear();
428 for (unsigned int i = 0; i < dst_corners.size(); i++) {
429 ip.set_uv(dst_corners[i].x, dst_corners[i].y);
430 corners.push_back(ip);
431 }
432
433 return corners;
434}
435
441void vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect &_modelROI)
442{
443 cv::Point2f ip;
444
445 ip.y = (float)_modelROI.y;
446 ip.x = (float)_modelROI.x;
447 ref_corners.push_back(ip);
448
449 ip.y = (float)(_modelROI.y + _modelROI.height);
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 + _modelROI.width);
455 ref_corners.push_back(ip);
456
457 ip.y = (float)_modelROI.y;
458 ip.x = (float)(_modelROI.x + _modelROI.width);
459 ref_corners.push_back(ip);
460}
461
462void vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint &_imPoint)
463{
464 if (_i >= refImagePoints.size()) {
465 throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
466 }
467 _imPoint = refImagePoints[_i];
468}
469
470void vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint &_referencePoint,
471 vpImagePoint &_currentPoint)
472{
473 // fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
474 if (_index >= currentImagePoints.size()) {
475 throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
476 }
477
478 _referencePoint = refImagePoints[_index];
479 _currentPoint = currentImagePoints[_index];
480}
481
482#elif !defined(VISP_BUILD_SHARED_LIBS)
483// Work around to avoid warning:
484// libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
485void dummy_vpPlanarObjectDetector() { };
486#endif
static const vpColor red
Definition vpColor.h:211
static const vpColor green
Definition vpColor.h:214
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 emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ fatalError
Fatal error.
Definition vpException.h:84
@ notInTheImage
Pixel not in the image.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
double get_j() const
void set_i(double ii)
double get_u() const
void set_uv(double u, double v)
double get_i() const
double get_v() 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 of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
Defines a rectangle in the plane.
Definition vpRect.h:76
double getWidth() const
Definition vpRect.h:224
double getLeft() const
Definition vpRect.h:170
vpImagePoint getTopLeft() const
Definition vpRect.h:196
double getHeight() const
Definition vpRect.h:163
double getTop() const
Definition vpRect.h:189
#define vpTRACE
Definition vpDebug.h:411