Visual Servoing Platform version 3.5.0
vpMbtDistanceLine.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 * Make the complete tracking of an object by using its CAD model
33 *
34 * Authors:
35 * Nicolas Melchior
36 * Romain Tallonneau
37 * Eric Marchand
38 *
39 *****************************************************************************/
40#include <visp3/core/vpConfig.h>
41
47#include <stdlib.h>
48#include <visp3/core/vpMeterPixelConversion.h>
49#include <visp3/core/vpPlane.h>
50#include <visp3/mbt/vpMbtDistanceLine.h>
51#include <visp3/visual_features/vpFeatureBuilder.h>
52
53void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
54void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);
55
60 : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
61 poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(), error(), nbFeature(), nbFeatureTotal(0),
62 Reinit(false), hiddenface(NULL), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
63{
64}
65
70{
71 // cout << "Deleting line " << index << endl;
72 if (line != NULL)
73 delete line;
74
75 for (unsigned int i = 0; i < meline.size(); i++)
76 if (meline[i] != NULL)
77 delete meline[i];
78
79 meline.clear();
80}
81
88void vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
89{
90 line->project(cMo);
91 p1->project(cMo);
92 p2->project(cMo);
93}
94
104void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
105{
106 vpColVector a(3);
107 vpColVector b(3);
108
109 // Calculate vector corresponding to PQ
110 a[0] = P.get_oX() - Q.get_oX();
111 a[1] = P.get_oY() - Q.get_oY();
112 a[2] = P.get_oZ() - Q.get_oZ();
113
114 // Calculate vector corresponding to PR
115 b[0] = P.get_oX() - R.get_oX();
116 b[1] = P.get_oY() - R.get_oY();
117 b[2] = P.get_oZ() - R.get_oZ();
118
119 // Calculate normal vector to plane PQ x PR
121
122 // Equation of the plane is given by:
123 double A = n[0];
124 double B = n[1];
125 double C = n[2];
126 double D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
127
128 double norm = sqrt(A * A + B * B + C * C);
129 plane.setA(A / norm);
130 plane.setB(B / norm);
131 plane.setC(C / norm);
132 plane.setD(D / norm);
133}
134
148void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
149{
150 vpPlane plane1;
151 vpPlane plane2;
152 buildPlane(P1, P2, P3, plane1);
153 buildPlane(P1, P2, P4, plane2);
154
155 L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), plane2.getA(), plane2.getB(),
156 plane2.getC(), plane2.getD());
157}
158
168{
169 if (line == NULL) {
170 line = new vpLine;
171 }
172
173 poly.setNbPoint(2);
174 poly.addPoint(0, _p1);
175 poly.addPoint(1, _p2);
176
177 p1 = &poly.p[0];
178 p2 = &poly.p[1];
179
180 vpColVector V1(3);
181 vpColVector V2(3);
182
183 V1[0] = p1->get_oX();
184 V1[1] = p1->get_oY();
185 V1[2] = p1->get_oZ();
186 V2[0] = p2->get_oX();
187 V2[1] = p2->get_oY();
188 V2[2] = p2->get_oZ();
189
190 // if((V1-V2).sumSquare()!=0)
191 if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
192 vpColVector V3(3);
193 V3[0] = double(rand_gen.next() % 1000) / 100;
194 V3[1] = double(rand_gen.next() % 1000) / 100;
195 V3[2] = double(rand_gen.next() % 1000) / 100;
196
197 vpColVector v_tmp1, v_tmp2;
198 v_tmp1 = V2 - V1;
199 v_tmp2 = V3 - V1;
200 vpColVector V4 = vpColVector::cross(v_tmp1, v_tmp2);
201
202 vpPoint P3(V3[0], V3[1], V3[2]);
203 vpPoint P4(V4[0], V4[1], V4[2]);
204 buildLine(*p1, *p2, P3, P4, *line);
205 } else {
206 vpPoint P3(V1[0], V1[1], V1[2]);
207 vpPoint P4(V2[0], V2[1], V2[2]);
208 buildLine(*p1, *p2, P3, P4, *line);
209 }
210}
211
218{
219 Lindex_polygon.push_back(idx);
220 Lindex_polygon_tracked.push_back(true);
221}
222
230void vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
231{
232 unsigned int ind = 0;
233 for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
234 if ((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) {
235 Lindex_polygon_tracked[ind] = track;
236 }
237 ind++;
238 }
239
240 isTrackedLine = false;
241 for (unsigned int i = 0; i < Lindex_polygon_tracked.size(); i++)
242 if (Lindex_polygon_tracked[i]) {
243 isTrackedLine = true;
244 break;
245 }
246
247 if (!isTrackedLine) {
248 isTrackedLineWithVisibility = false;
249 return;
250 }
251
253}
254
261{
262 if (!isTrackedLine) {
263 isTrackedLineWithVisibility = false;
264 return;
265 }
266
267 unsigned int ind = 0;
268 isTrackedLineWithVisibility = false;
269 for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
270 if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) {
271 isTrackedLineWithVisibility = true;
272 break;
273 }
274 ind++;
275 }
276}
277
284{
285 me = _me;
286
287 for (unsigned int i = 0; i < meline.size(); i++)
288 if (meline[i] != NULL) {
289 // nbFeature[i] = 0;
290 meline[i]->reset();
291 meline[i]->setMe(me);
292 }
293
294 // nbFeatureTotal = 0;
295}
296
309 const vpImage<bool> *mask)
310{
311 for (unsigned int i = 0; i < meline.size(); i++) {
312 if (meline[i] != NULL)
313 delete meline[i];
314 }
315
316 meline.clear();
317 nbFeature.clear();
318 nbFeatureTotal = 0;
319
320 if (isvisible) {
321 p1->changeFrame(cMo);
322 p2->changeFrame(cMo);
323
324 if (poly.getClipping() > 3) // Contains at least one FOV constraint
325 cam.computeFov(I.getWidth(), I.getHeight());
326
327 poly.computePolygonClipped(cam);
328
329 if (poly.polyClipped.size() == 2) { // Les points sont visibles.
330
331 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
332
333 if (useScanLine) {
334 hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
335 } else {
336 linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
337 }
338
339 if (linesLst.size() == 0) {
340 return false;
341 }
342
343 line->changeFrame(cMo);
344 try {
345 line->projection();
346 }
347 catch(...) {
348 isvisible = false;
349 return false;
350 }
351 double rho, theta;
352 // rho theta uv
354
355 while (theta > M_PI) {
356 theta -= M_PI;
357 }
358 while (theta < -M_PI) {
359 theta += M_PI;
360 }
361
362 if (theta < -M_PI / 2.0)
363 theta = -theta - 3 * M_PI / 2.0;
364 else
365 theta = M_PI / 2.0 - theta;
366
367 for (unsigned int i = 0; i < linesLst.size(); i++) {
368 vpImagePoint ip1, ip2;
369
370 linesLst[i].first.project();
371 linesLst[i].second.project();
372
373 vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
374 vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
375
376 vpMbtMeLine *melinePt = new vpMbtMeLine;
377 melinePt->setMask(*mask);
378 melinePt->setMe(me);
379
380 melinePt->setInitRange(0);
381
382 int marge = /*10*/ 5; // ou 5 normalement
383 if (ip1.get_j() < ip2.get_j()) {
384 melinePt->jmin = (int)ip1.get_j() - marge;
385 melinePt->jmax = (int)ip2.get_j() + marge;
386 } else {
387 melinePt->jmin = (int)ip2.get_j() - marge;
388 melinePt->jmax = (int)ip1.get_j() + marge;
389 }
390 if (ip1.get_i() < ip2.get_i()) {
391 melinePt->imin = (int)ip1.get_i() - marge;
392 melinePt->imax = (int)ip2.get_i() + marge;
393 } else {
394 melinePt->imin = (int)ip2.get_i() - marge;
395 melinePt->imax = (int)ip1.get_i() + marge;
396 }
397
398 try {
399 melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
400 meline.push_back(melinePt);
401 nbFeature.push_back((unsigned int) melinePt->getMeList().size());
402 nbFeatureTotal += nbFeature.back();
403 } catch (...) {
404 delete melinePt;
405 isvisible = false;
406 return false;
407 }
408 }
409 } else {
410 isvisible = false;
411 }
412 }
413
414 return true;
415}
416
423{
424 if (isvisible) {
425 try {
426 nbFeature.clear();
427 nbFeatureTotal = 0;
428 for (size_t i = 0; i < meline.size(); i++) {
429 meline[i]->track(I);
430 nbFeature.push_back((unsigned int)meline[i]->getMeList().size());
431 nbFeatureTotal += (unsigned int)meline[i]->getMeList().size();
432 }
433 } catch (...) {
434 for (size_t i = 0; i < meline.size(); i++) {
435 if (meline[i] != NULL)
436 delete meline[i];
437 }
438
439 nbFeature.clear();
440 meline.clear();
441 nbFeatureTotal = 0;
442 Reinit = true;
443 isvisible = false;
444 }
445 }
446}
447
455{
456 if (isvisible) {
457 p1->changeFrame(cMo);
458 p2->changeFrame(cMo);
459
460 if (poly.getClipping() > 3) // Contains at least one FOV constraint
461 cam.computeFov(I.getWidth(), I.getHeight());
462
463 poly.computePolygonClipped(cam);
464
465 if (poly.polyClipped.size() == 2) { // Les points sont visibles.
466
467 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
468
469 if (useScanLine) {
470 hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
471 } else {
472 linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
473 }
474
475 if (linesLst.size() != meline.size() || linesLst.size() == 0) {
476 for (size_t i = 0; i < meline.size(); i++) {
477 if (meline[i] != NULL)
478 delete meline[i];
479 }
480
481 meline.clear();
482 nbFeature.clear();
483 nbFeatureTotal = 0;
484 isvisible = false;
485 Reinit = true;
486 } else {
487 line->changeFrame(cMo);
488 try {
489 line->projection();
490 }
491 catch(...) {
492 for (size_t j = 0; j < meline.size(); j++) {
493 if (meline[j] != NULL)
494 delete meline[j];
495 }
496
497 meline.clear();
498 nbFeature.clear();
499 nbFeatureTotal = 0;
500 isvisible = false;
501 Reinit = true;
502 return;
503 }
504 double rho, theta;
505 // rho theta uv
507
508 while (theta > M_PI) {
509 theta -= M_PI;
510 }
511 while (theta < -M_PI) {
512 theta += M_PI;
513 }
514
515 if (theta < -M_PI / 2.0)
516 theta = -theta - 3 * M_PI / 2.0;
517 else
518 theta = M_PI / 2.0 - theta;
519
520 try {
521 for (unsigned int i = 0; i < linesLst.size(); i++) {
522 vpImagePoint ip1, ip2;
523
524 linesLst[i].first.project();
525 linesLst[i].second.project();
526
527 vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
528 vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
529
530 int marge = /*10*/ 5; // ou 5 normalement
531 if (ip1.get_j() < ip2.get_j()) {
532 meline[i]->jmin = (int)ip1.get_j() - marge;
533 meline[i]->jmax = (int)ip2.get_j() + marge;
534 } else {
535 meline[i]->jmin = (int)ip2.get_j() - marge;
536 meline[i]->jmax = (int)ip1.get_j() + marge;
537 }
538 if (ip1.get_i() < ip2.get_i()) {
539 meline[i]->imin = (int)ip1.get_i() - marge;
540 meline[i]->imax = (int)ip2.get_i() + marge;
541 } else {
542 meline[i]->imin = (int)ip2.get_i() - marge;
543 meline[i]->imax = (int)ip1.get_i() + marge;
544 }
545
546 meline[i]->updateParameters(I, ip1, ip2, rho, theta);
547 nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
549 }
550 } catch (...) {
551 for (size_t j = 0; j < meline.size(); j++) {
552 if (meline[j] != NULL)
553 delete meline[j];
554 }
555
556 meline.clear();
557 nbFeature.clear();
558 nbFeatureTotal = 0;
559 isvisible = false;
560 Reinit = true;
561 }
562 }
563 } else {
564 for (size_t i = 0; i < meline.size(); i++) {
565 if (meline[i] != NULL)
566 delete meline[i];
567 }
568 nbFeature.clear();
569 meline.clear();
570 nbFeatureTotal = 0;
571 isvisible = false;
572 }
573 }
574}
575
587{
588 for (size_t i = 0; i < meline.size(); i++) {
589 if (meline[i] != NULL)
590 delete meline[i];
591 }
592
593 nbFeature.clear();
594 meline.clear();
595 nbFeatureTotal = 0;
596
597 if (!initMovingEdge(I, cMo, false, mask))
598 Reinit = true;
599
600 Reinit = false;
601}
602
615 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
616 bool displayFullModel)
617{
618 std::vector<std::vector<double> > models =
619 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
620
621 for (size_t i = 0; i < models.size(); i++) {
622 vpImagePoint ip1(models[i][1], models[i][2]);
623 vpImagePoint ip2(models[i][3], models[i][4]);
624 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
625 }
626}
627
640 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
641 bool displayFullModel)
642{
643 std::vector<std::vector<double> > models =
644 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
645
646 for (size_t i = 0; i < models.size(); i++) {
647 vpImagePoint ip1(models[i][1], models[i][2]);
648 vpImagePoint ip2(models[i][3], models[i][4]);
649 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
650 }
651}
652
668{
669 for (size_t i = 0; i < meline.size(); i++) {
670 if (meline[i] != NULL) {
671 meline[i]->display(I);
672 }
673 }
674}
675
677{
678 for (size_t i = 0; i < meline.size(); i++) {
679 if (meline[i] != NULL) {
680 meline[i]->display(I);
681 }
682 }
683}
684
689std::vector<std::vector<double> > vpMbtDistanceLine::getFeaturesForDisplay()
690{
691 std::vector<std::vector<double> > features;
692
693 for (size_t i = 0; i < meline.size(); i++) {
694 vpMbtMeLine *me_l = meline[i];
695 if (me_l != NULL) {
696 for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
697 vpMeSite p_me_l = *it;
698#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
699 std::vector<double> params = {0, //ME
700 p_me_l.get_ifloat(),
701 p_me_l.get_jfloat(),
702 static_cast<double>(p_me_l.getState())};
703#else
704 std::vector<double> params;
705 params.push_back(0); // ME
706 params.push_back(p_me_l.get_ifloat());
707 params.push_back(p_me_l.get_jfloat());
708 params.push_back(static_cast<double>(p_me_l.getState()));
709#endif
710 features.push_back(params);
711 }
712 }
713 }
714
715 return features;
716}
717
729std::vector<std::vector<double> > vpMbtDistanceLine::getModelForDisplay(unsigned int width, unsigned int height,
730 const vpHomogeneousMatrix &cMo,
731 const vpCameraParameters &camera,
732 bool displayFullModel)
733{
734 std::vector<std::vector<double> > models;
735
736 if ((isvisible && isTrackedLine) || displayFullModel) {
737 p1->changeFrame(cMo);
738 p2->changeFrame(cMo);
739
740 vpImagePoint ip1, ip2;
741 vpCameraParameters c = camera;
742 if (poly.getClipping() > 3) // Contains at least one FOV constraint
743 c.computeFov(width, height);
744
745 poly.computePolygonClipped(c);
746
747 if (poly.polyClipped.size() == 2 &&
748 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
749 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
750 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
751 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
752 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
753 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
754
755 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
756 if (useScanLine && !displayFullModel) {
757 hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
758 } else {
759 linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
760 }
761
762 for (unsigned int i = 0; i < linesLst.size(); i++) {
763 linesLst[i].first.project();
764 linesLst[i].second.project();
765
766 vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
767 vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
768
769#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
770 std::vector<double> params = {0, //0 for line parameters
771 ip1.get_i(),
772 ip1.get_j(),
773 ip2.get_i(),
774 ip2.get_j()};
775#else
776 std::vector<double> params;
777 params.push_back(0); //0 for line parameters
778 params.push_back(ip1.get_i());
779 params.push_back(ip1.get_j());
780 params.push_back(ip2.get_i());
781 params.push_back(ip2.get_j());
782#endif
783 models.push_back(params);
784 }
785 }
786 }
787
788 return models;
789}
790
795{
796 if (isvisible) {
799 } else {
800 for (size_t i = 0; i < meline.size(); i++) {
801 nbFeature[i] = 0;
802 // To be consistent with nbFeature[i] = 0
803 std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
804 me_site_list.clear();
805 }
806 nbFeatureTotal = 0;
807 }
808}
809
815{
816 if (isvisible) {
817 try {
818 // feature projection
819 line->changeFrame(cMo);
820 line->projection();
821
822 vpFeatureBuilder::create(featureline, *line);
823
824 double rho = featureline.getRho();
825 double theta = featureline.getTheta();
826
827 double co = cos(theta);
828 double si = sin(theta);
829
830 double mx = 1.0 / cam.get_px();
831 double my = 1.0 / cam.get_py();
832 double xc = cam.get_u0();
833 double yc = cam.get_v0();
834
835 double alpha_;
836 vpMatrix H = featureline.interaction();
837
838 double x, y;
839 unsigned int j = 0;
840
841 for (size_t i = 0; i < meline.size(); i++) {
842 for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
843 it != meline[i]->getMeList().end(); ++it) {
844 x = (double)it->j;
845 y = (double)it->i;
846
847 x = (x - xc) * mx;
848 y = (y - yc) * my;
849
850 alpha_ = x * si - y * co;
851
852 double *Lrho = H[0];
853 double *Ltheta = H[1];
854 // Calculate interaction matrix for a distance
855 for (unsigned int k = 0; k < 6; k++) {
856 L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
857 }
858 error[j] = rho - (x * co + y * si);
859 j++;
860 }
861 }
862 } catch (...) {
863 // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
864 // Set the corresponding interaction matrix part to zero
865 unsigned int j = 0;
866 for (size_t i = 0; i < meline.size(); i++) {
867 for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
868 it != meline[i]->getMeList().end(); ++it) {
869 for (unsigned int k = 0; k < 6; k++) {
870 L[j][k] = 0.0;
871 }
872
873 error[j] = 0.0;
874 j++;
875 }
876 }
877 }
878 }
879}
880
889bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
890{
891 if (threshold > I.getWidth() || threshold > I.getHeight()) {
892 return true;
893 }
894 if (isvisible) {
895
896 for (size_t i = 0; i < meline.size(); i++) {
897 for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
898 ++it) {
899 int i_ = it->i;
900 int j_ = it->j;
901
902 if (i_ < 0 || j_ < 0) { // out of image.
903 return true;
904 }
905
906 if (((unsigned int)i_ > (I.getHeight() - threshold)) || (unsigned int)i_ < threshold ||
907 ((unsigned int)j_ > (I.getWidth() - threshold)) || (unsigned int)j_ < threshold) {
908 return true;
909 }
910 }
911 }
912 }
913 return false;
914}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:351
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
double getTheta() const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
double getRho() const
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
double get_j() const
Definition: vpImagePoint.h:214
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:105
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpLine.cpp:330
double getRho() const
Definition: vpLine.h:134
void projection()
Definition: vpLine.cpp:193
double getTheta() const
Definition: vpLine.h:146
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void displayMovingEdges(const vpImage< unsigned char > &I)
std::vector< bool > Lindex_polygon_tracked
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
bool isvisible
Indicates if the line is visible or not.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpPoint * p2
The second extremity.
vpLine * line
The 3D line.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
std::string getName() const
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getFeaturesForDisplay()
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setTracked(const std::string &name, const bool &track)
void addPolygon(const int &index)
void trackMovingEdge(const vpImage< unsigned char > &I)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
vpMeSiteState getState() const
Definition: vpMeSite.h:190
double get_ifloat() const
Definition: vpMeSite.h:160
double get_jfloat() const
Definition: vpMeSite.h:167
Definition: vpMe.h:61
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 &x, const double &y, double &u, double &v)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
void setA(double a)
Definition: vpPlane.h:82
void setD(double d)
Definition: vpPlane.h:88
double getD() const
Definition: vpPlane.h:108
void setC(double c)
Definition: vpPlane.h:86
double getA() const
Definition: vpPlane.h:102
double getC() const
Definition: vpPlane.h:106
double getB() const
Definition: vpPlane.h:104
void setB(double b)
Definition: vpPlane.h:84
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_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
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
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
virtual void setNbPoint(unsigned int nb)
unsigned int getClipping() const
Definition: vpPolygon3D.h:118
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition: vpPolygon3D.h:83
void addPoint(unsigned int n, const vpPoint &P)
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101
uint32_t next()
Definition: vpUniRand.cpp:149