Visual Servoing Platform version 3.5.0
vpMbtDistanceKltCylinder.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 * Klt cylinder, containing points of interest.
33 *
34 * Authors:
35 * Aurelien Yol
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpPolygon.h>
40#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
41#include <visp3/mbt/vpMbtDistanceKltPoints.h>
42
43#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
44
45#if defined(VISP_HAVE_CLIPPER)
46#include <clipper.hpp> // clipper private library
47#endif
48
49#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
50#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
51#endif
52
58 : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(), initPoints(), initPoints3D(), curPoints(),
59 curPointsInd(), nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), cam(),
60 isTrackedKltCylinder(true), listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
61{
62}
63
69
70void vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
71{
72 p1Ext = p1;
73 p2Ext = p2;
74
75 vpColVector ABC(3);
76 vpColVector V1(3);
77 vpColVector V2(3);
78
79 V1[0] = p1.get_oX();
80 V1[1] = p1.get_oY();
81 V1[2] = p1.get_oZ();
82 V2[0] = p2.get_oX();
83 V2[1] = p2.get_oY();
84 V2[2] = p2.get_oZ();
85
86 // Get the axis of the cylinder
87 ABC = V1 - V2;
88
89 // Build our extremity circles
90 circle1.setWorldCoordinates(ABC[0], ABC[1], ABC[2], p1.get_oX(), p1.get_oY(), p1.get_oZ(), r);
91 circle2.setWorldCoordinates(ABC[0], ABC[1], ABC[2], p2.get_oX(), p2.get_oY(), p2.get_oZ(), r);
92
93 // Build our cylinder
94 cylinder.setWorldCoordinates(ABC[0], ABC[1], ABC[2], (p1.get_oX() + p2.get_oX()) / 2.0,
95 (p1.get_oY() + p2.get_oY()) / 2.0, (p1.get_oZ() + p2.get_oZ()) / 2.0, r);
96}
97
107{
108 c0Mo = cMo;
109 cylinder.changeFrame(cMo);
110
111 // extract ids of the points in the face
112 nbPointsInit = 0;
113 nbPointsCur = 0;
114 initPoints = std::map<int, vpImagePoint>();
115 initPoints3D = std::map<int, vpPoint>();
116 curPoints = std::map<int, vpImagePoint>();
117 curPointsInd = std::map<int, int>();
118
119 for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
120 long id;
121 float x_tmp, y_tmp;
122 _tracker.getFeature((int)i, id, x_tmp, y_tmp);
123
124 bool add = false;
125
126 if (useScanLine) {
127 if ((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
128 (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth()) {
129 for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++)
130 if (hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] ==
132 add = true;
133 break;
134 }
135 }
136 } else {
137 std::vector<vpImagePoint> roi;
138 for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) {
139 hiddenface->getPolygon()[(size_t)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
140 if (vpPolygon::isInside(roi, y_tmp, x_tmp)) {
141 add = true;
142 break;
143 }
144 roi.clear();
145 }
146 }
147
148 if (add) {
149
150 double xm = 0, ym = 0;
151 vpPixelMeterConversion::convertPoint(cam, x_tmp, y_tmp, xm, ym);
152 double Z = computeZ(xm, ym);
153 if (!vpMath::isNaN(Z)) {
154#if TARGET_OS_IPHONE
155 initPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
156 curPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
157 curPointsInd[(int)id] = (int)i;
158#else
159 initPoints[id] = vpImagePoint(y_tmp, x_tmp);
160 curPoints[id] = vpImagePoint(y_tmp, x_tmp);
161 curPointsInd[id] = (int)i;
162#endif
163 nbPointsInit++;
164 nbPointsCur++;
165
166 vpPoint p;
167 p.setWorldCoordinates(xm * Z, ym * Z, Z);
168#if TARGET_OS_IPHONE
169 initPoints3D[(int)id] = p;
170#else
171 initPoints3D[id] = p;
172#endif
173 // std::cout << "Computed Z for : " << xm << "," << ym << " : " <<
174 // computeZ(xm,ym) << std::endl;
175 }
176 }
177 }
178
179 if (nbPointsCur >= minNbPoint)
180 enoughPoints = true;
181 else
182 enoughPoints = false;
183
184 // std::cout << "Nb detected points in cylinder : " << nbPointsCur <<
185 // std::endl;
186}
187
197{
198 long id;
199 float x, y;
200 nbPointsCur = 0;
201 curPoints = std::map<int, vpImagePoint>();
202 curPointsInd = std::map<int, int>();
203
204 for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
205 _tracker.getFeature((int)i, id, x, y);
206 if (isTrackedFeature((int)id)) {
207#if TARGET_OS_IPHONE
208 curPoints[(int)id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
209 curPointsInd[(int)id] = (int)i;
210#else
211 curPoints[id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
212 curPointsInd[id] = (int)i;
213#endif
214 nbPointsCur++;
215 }
216 }
217
218 if (nbPointsCur >= minNbPoint)
219 enoughPoints = true;
220 else
221 enoughPoints = false;
222
223 return nbPointsCur;
224}
225
234void vpMbtDistanceKltCylinder::removeOutliers(const vpColVector &_w, const double &threshold_outlier)
235{
236 std::map<int, vpImagePoint> tmp;
237 std::map<int, int> tmp2;
238 unsigned int nbSupp = 0;
239 unsigned int k = 0;
240
241 nbPointsCur = 0;
242 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
243 for (; iter != curPoints.end(); ++iter) {
244 if (_w[k] > threshold_outlier && _w[k + 1] > threshold_outlier) {
245 // if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
246 tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
247 tmp2[iter->first] = curPointsInd[iter->first];
248 nbPointsCur++;
249 } else {
250 nbSupp++;
251 initPoints.erase(iter->first);
252 }
253
254 k += 2;
255 }
256
257 if (nbSupp != 0) {
258 curPoints = std::map<int, vpImagePoint>();
259 curPointsInd = std::map<int, int>();
260
261 curPoints = tmp;
262 curPointsInd = tmp2;
263 if (nbPointsCur >= minNbPoint)
264 enoughPoints = true;
265 else
266 enoughPoints = false;
267 }
268}
269
282 vpMatrix &_J)
283{
284 unsigned int index_ = 0;
285
286 cylinder.changeFrame(_cMc0 * c0Mo);
287
288 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
289 for (; iter != curPoints.end(); ++iter) {
290 int id(iter->first);
291 double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
292
293 double x_cur(0), y_cur(0);
294 vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
295
296 vpPoint p0 = initPoints3D[id];
297 p0.changeFrame(_cMc0);
298 p0.project();
299
300 double x0_transform(p0.get_x()), y0_transform(p0.get_y());
301
302 double Z = computeZ(x_cur, y_cur);
303
304 if (vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()) {
305 // std::cout << "Z is Nan : " << A << " , " << B << " , " << C << "
306 // | " << Z << " | " << x_cur << " , " << y_cur << std::endl;
307 // std::cout << std::sqrt(B*B - A*C) << " , " << B*B - A*C <<
308 // std::endl;
309
310 _J[2 * index_][0] = 0;
311 _J[2 * index_][1] = 0;
312 _J[2 * index_][2] = 0;
313 _J[2 * index_][3] = 0;
314 _J[2 * index_][4] = 0;
315 _J[2 * index_][5] = 0;
316
317 _J[2 * index_ + 1][0] = 0;
318 _J[2 * index_ + 1][1] = 0;
319 _J[2 * index_ + 1][2] = 0;
320 _J[2 * index_ + 1][3] = 0;
321 _J[2 * index_ + 1][4] = 0;
322 _J[2 * index_ + 1][5] = 0;
323
324 _R[2 * index_] = (x0_transform - x_cur);
325 _R[2 * index_ + 1] = (y0_transform - y_cur);
326 index_++;
327 } else {
328 double invZ = 1.0 / Z;
329
330 _J[2 * index_][0] = -invZ;
331 _J[2 * index_][1] = 0;
332 _J[2 * index_][2] = x_cur * invZ;
333 _J[2 * index_][3] = x_cur * y_cur;
334 _J[2 * index_][4] = -(1 + x_cur * x_cur);
335 _J[2 * index_][5] = y_cur;
336
337 _J[2 * index_ + 1][0] = 0;
338 _J[2 * index_ + 1][1] = -invZ;
339 _J[2 * index_ + 1][2] = y_cur * invZ;
340 _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
341 _J[2 * index_ + 1][4] = -y_cur * x_cur;
342 _J[2 * index_ + 1][5] = -x_cur;
343
344 _R[2 * index_] = (x0_transform - x_cur);
345 _R[2 * index_ + 1] = (y0_transform - y_cur);
346 index_++;
347 }
348 }
349}
350
358bool vpMbtDistanceKltCylinder::isTrackedFeature(int _id)
359{
360 std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
361 if (iter != initPoints.end())
362 return true;
363
364 return false;
365}
366
377#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
378 cv::Mat &mask,
379#else
380 IplImage *mask,
381#endif
382 unsigned char nb, unsigned int shiftBorder)
383{
384#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
385 int width = mask.cols;
386 int height = mask.rows;
387#else
388 int width = mask->width;
389 int height = mask->height;
390#endif
391
392 for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) {
393 if ((*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->isVisible() &&
394 (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getNbPoint() > 2) {
395 int i_min, i_max, j_min, j_max;
396 std::vector<vpImagePoint> roi;
397 (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
398
399 double shiftBorder_d = (double)shiftBorder;
400#if defined(VISP_HAVE_CLIPPER)
401 std::vector<vpImagePoint> roi_offset;
402
403 ClipperLib::Path path;
404 for (std::vector<vpImagePoint>::const_iterator it = roi.begin(); it != roi.end(); ++it) {
405 path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v()));
406 }
407
408 ClipperLib::Paths solution;
409 ClipperLib::ClipperOffset co;
410 co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
411 co.Execute(solution, -shiftBorder_d);
412
413 // Keep biggest polygon by area
414 if (!solution.empty()) {
415 size_t index_max = 0;
416
417 if (solution.size() > 1) {
418 double max_area = 0;
419 vpPolygon polygon_area;
420
421 for (size_t i = 0; i < solution.size(); i++) {
422 std::vector<vpImagePoint> corners;
423
424 for (size_t j = 0; j < solution[i].size(); j++) {
425 corners.push_back(vpImagePoint((double)(solution[i][j].Y), (double)(solution[i][j].X)));
426 }
427
428 polygon_area.buildFrom(corners);
429 if (polygon_area.getArea() > max_area) {
430 max_area = polygon_area.getArea();
431 index_max = i;
432 }
433 }
434 }
435
436 for (size_t i = 0; i < solution[index_max].size(); i++) {
437 roi_offset.push_back(vpImagePoint((double)(solution[index_max][i].Y), (double)(solution[index_max][i].X)));
438 }
439 } else {
440 roi_offset = roi;
441 }
442
443 vpPolygon polygon_test(roi_offset);
444 vpImagePoint imPt;
445#endif
446
447#if defined(VISP_HAVE_CLIPPER)
448 vpPolygon3D::getMinMaxRoi(roi_offset, i_min, i_max, j_min, j_max);
449#else
450 vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min, j_max);
451#endif
452
453 /* check image boundaries */
454 if (i_min > height) { // underflow
455 i_min = 0;
456 }
457 if (i_max > height) {
458 i_max = height;
459 }
460 if (j_min > width) { // underflow
461 j_min = 0;
462 }
463 if (j_max > width) {
464 j_max = width;
465 }
466
467#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
468 for (int i = i_min; i < i_max; i++) {
469 double i_d = (double)i;
470
471 for (int j = j_min; j < j_max; j++) {
472 double j_d = (double)j;
473
474#if defined(VISP_HAVE_CLIPPER)
475 imPt.set_ij(i_d, j_d);
476 if (polygon_test.isInside(imPt)) {
477 mask.ptr<uchar>(i)[j] = nb;
478 }
479#else
480 if (shiftBorder != 0) {
481 if (vpPolygon::isInside(roi, i_d, j_d) &&
482 vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d + shiftBorder_d) &&
483 vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d + shiftBorder_d) &&
484 vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d - shiftBorder_d) &&
485 vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d - shiftBorder_d)) {
486 mask.at<unsigned char>(i, j) = nb;
487 }
488 } else {
489 if (vpPolygon::isInside(roi, i, j)) {
490 mask.at<unsigned char>(i, j) = nb;
491 }
492 }
493#endif
494 }
495 }
496#else
497 unsigned char *ptrData = (unsigned char *)mask->imageData + i_min * mask->widthStep + j_min;
498 for (int i = i_min; i < i_max; i++) {
499 double i_d = (double)i;
500 for (int j = j_min; j < j_max; j++) {
501 double j_d = (double)j;
502 if (shiftBorder != 0) {
503 if (vpPolygon::isInside(roi, i_d, j_d) &&
504 vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d + shiftBorder_d) &&
505 vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d + shiftBorder_d) &&
506 vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d - shiftBorder_d) &&
507 vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d - shiftBorder_d)) {
508 *(ptrData++) = nb;
509 } else {
510 ptrData++;
511 }
512 } else {
513 if (vpPolygon::isInside(roi, i, j)) {
514 *(ptrData++) = nb;
515 } else {
516 ptrData++;
517 }
518 }
519 }
520 ptrData += mask->widthStep - j_max + j_min;
521 }
522#endif
523 }
524 }
525}
526
533{
534 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
535 for (; iter != curPoints.end(); ++iter) {
536 int id(iter->first);
537 vpImagePoint iP;
538 iP.set_i(static_cast<double>(iter->second.get_i()));
539 iP.set_j(static_cast<double>(iter->second.get_j()));
540
542
543 iP.set_i(vpMath::round(iP.get_i() + 7));
544 iP.set_j(vpMath::round(iP.get_j() + 7));
545 std::stringstream ss;
546 ss << id;
547 vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
548 }
549}
550
557{
558 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
559 for (; iter != curPoints.end(); ++iter) {
560 int id(iter->first);
561 vpImagePoint iP;
562 iP.set_i(static_cast<double>(iter->second.get_i()));
563 iP.set_j(static_cast<double>(iter->second.get_j()));
564
566
567 iP.set_i(vpMath::round(iP.get_i() + 7));
568 iP.set_j(vpMath::round(iP.get_j() + 7));
569 std::stringstream ss;
570 ss << id;
571 vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
572 }
573}
574
576 const vpCameraParameters &camera, const vpColor &col,
577 unsigned int thickness, const bool /*displayFullModel*/)
578{
579 std::vector<std::vector<double> > models = getModelForDisplay(cMo, camera);
580
581 for (size_t i = 0; i < models.size(); i++) {
582 vpImagePoint ip1(models[i][1], models[i][2]);
583 vpImagePoint ip2(models[i][3], models[i][4]);
584 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
585 }
586}
587
589 const vpCameraParameters &camera, const vpColor &col,
590 unsigned int thickness, const bool /*displayFullModel*/)
591{
592 std::vector<std::vector<double> > models = getModelForDisplay(cMo, camera);
593
594 for (size_t i = 0; i < models.size(); i++) {
595 vpImagePoint ip1(models[i][1], models[i][2]);
596 vpImagePoint ip2(models[i][3], models[i][4]);
597
598 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
599 }
600}
601
607std::vector<std::vector<double> > vpMbtDistanceKltCylinder::getFeaturesForDisplay()
608{
609 std::vector<std::vector<double> > features;
610
611 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
612 for (; iter != curPoints.end(); ++iter) {
613 int id(iter->first);
614 vpImagePoint iP;
615 iP.set_i(static_cast<double>(iter->second.get_i()));
616 iP.set_j(static_cast<double>(iter->second.get_j()));
617
618 vpImagePoint iP2;
619 iP2.set_i(vpMath::round(iP.get_i() + 7));
620 iP2.set_j(vpMath::round(iP.get_j() + 7));
621
622#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
623 std::vector<double> params = {1, //KLT
624 iP.get_i(),
625 iP.get_j(),
626 iP2.get_i(),
627 iP2.get_j(),
628 static_cast<double>(id)};
629#else
630 std::vector<double> params;
631 params.push_back(1); //KLT
632 params.push_back(iP.get_i());
633 params.push_back(iP.get_j());
634 params.push_back(iP2.get_i());
635 params.push_back(iP2.get_j());
636 params.push_back(static_cast<double>(id));
637#endif
638 features.push_back(params);
639 }
640
641 return features;
642}
643
652std::vector<std::vector<double> > vpMbtDistanceKltCylinder::getModelForDisplay(const vpHomogeneousMatrix &cMo,
653 const vpCameraParameters &camera)
654{
655 std::vector<std::vector<double> > models;
656
657 // if(isvisible || displayFullModel)
658 {
659 // Perspective projection
660 circle1.changeFrame(cMo);
661 circle2.changeFrame(cMo);
662 cylinder.changeFrame(cMo);
663
664 try {
665 circle1.projection();
666 } catch (...) {
667 std::cout << "Problem projection circle 1";
668 }
669 try {
670 circle2.projection();
671 } catch (...) {
672 std::cout << "Problem projection circle 2";
673 }
674
675 cylinder.projection();
676
677 double rho1, theta1;
678 double rho2, theta2;
679
680 // Meters to pixels conversion
681 vpMeterPixelConversion::convertLine(camera, cylinder.getRho1(), cylinder.getTheta1(), rho1, theta1);
682 vpMeterPixelConversion::convertLine(camera, cylinder.getRho2(), cylinder.getTheta2(), rho2, theta2);
683
684 // Determine intersections between circles and limbos
685 double i11, i12, i21, i22, j11, j12, j21, j22;
686
687 vpCircle::computeIntersectionPoint(circle1, camera, rho1, theta1, i11, j11);
688 vpCircle::computeIntersectionPoint(circle2, camera, rho1, theta1, i12, j12);
689
690 vpCircle::computeIntersectionPoint(circle1, camera, rho2, theta2, i21, j21);
691 vpCircle::computeIntersectionPoint(circle2, camera, rho2, theta2, i22, j22);
692
693 // Create the image points
694 vpImagePoint ip11, ip12, ip21, ip22;
695 ip11.set_ij(i11, j11);
696 ip12.set_ij(i12, j12);
697 ip21.set_ij(i21, j21);
698 ip22.set_ij(i22, j22);
699
700#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
701 std::vector<double> params1 = {0, //line parameters
702 ip11.get_i(),
703 ip11.get_j(),
704 ip12.get_i(),
705 ip12.get_j()};
706 models.push_back(params1);
707
708 std::vector<double> params2 = {0, //line parameters
709 ip21.get_i(),
710 ip21.get_j(),
711 ip22.get_i(),
712 ip22.get_j()};
713#else
714 std::vector<double> params1, params2;
715 params1.push_back(0); //line parameters
716 params1.push_back(ip11.get_i());
717 params1.push_back(ip11.get_j());
718 params1.push_back(ip12.get_i());
719 params1.push_back(ip12.get_j());
720
721 params2.push_back(0); //line parameters
722 params2.push_back(ip21.get_i());
723 params2.push_back(ip21.get_j());
724 params2.push_back(ip22.get_i());
725 params2.push_back(ip22.get_j());
726#endif
727 models.push_back(params1);
728 models.push_back(params2);
729 }
730
731 return models;
732}
733
734// ######################
735// Private Functions
736// ######################
737
738double vpMbtDistanceKltCylinder::computeZ(const double &x, const double &y)
739{
740 // double A = x*x + y*y + 1 -
741 // ((cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()) *
742 // (cylinder.getA()*x+cylinder.getB()*y+cylinder.getC())); double B = (x *
743 // cylinder.getX() + y * cylinder.getY() + cylinder.getZ()); double C =
744 // cylinder.getX() * cylinder.getX() + cylinder.getY() * cylinder.getY() +
745 // cylinder.getZ() * cylinder.getZ() - cylinder.getR() * cylinder.getR();
746 //
747 // return (B - std::sqrt(B*B - A*C))/A;
748
749 return cylinder.computeZ(x, y);
750}
751#elif !defined(VISP_BUILD_SHARED_LIBS)
752// Work arround to avoid warning:
753// libvisp_mbt.a(vpMbtDistanceKltCylinder.cpp.o) has no symbols
754void dummy_vpMbtDistanceKltCylinder(){};
755#endif
Generic class defining intrinsic camera parameters.
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
Definition: vpCircle.cpp:248
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:60
void projection()
Definition: vpCircle.cpp:140
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:398
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
double getRho1() const
Definition: vpCylinder.h:136
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCylinder.cpp:249
void projection()
Definition: vpCylinder.cpp:154
double getTheta1() const
Definition: vpCylinder.h:142
double computeZ(double x, double y) const
Definition: vpCylinder.cpp:344
double getTheta2() const
Definition: vpCylinder.h:155
double getRho2() const
Definition: vpCylinder.h:149
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:65
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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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_ij(double ii, double jj)
Definition: vpImagePoint.h:188
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_i() const
Definition: vpImagePoint.h:203
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
void getFeature(const int &index, long &id, float &x, float &y) const
static bool isNaN(double value)
Definition: vpMath.cpp:85
static int round(double x)
Definition: vpMath.h:247
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
std::vector< PolygonType * > & getPolygon()
vpMbScanLine & getMbScanLineRenderer()
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
std::vector< std::vector< double > > getFeaturesForDisplay()
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
bool useScanLine
Use scanline rendering.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void displayPrimitive(const vpImage< unsigned char > &_I)
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
Defines a generic 2D polygon.
Definition: vpPolygon.h:104
double getArea() const
Definition: vpPolygon.h:161
void buildFrom(const std::vector< vpImagePoint > &corners)
Definition: vpPolygon.cpp:131
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Definition: vpPolygon.cpp:309