Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbtDistanceLine.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 * Romain Tallonneau
36 *
37*****************************************************************************/
38#include <visp3/core/vpConfig.h>
39
45#include <stdlib.h>
46#include <visp3/core/vpMeterPixelConversion.h>
47#include <visp3/core/vpPlane.h>
48#include <visp3/mbt/vpMbtDistanceLine.h>
49#include <visp3/visual_features/vpFeatureBuilder.h>
50
51void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
52void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);
53
58 : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
59 poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(), error(), nbFeature(), nbFeatureTotal(0),
60 Reinit(false), hiddenface(NULL), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
61{
62}
63
68{
69 if (line != NULL)
70 delete line;
71
72 for (unsigned int i = 0; i < meline.size(); i++)
73 if (meline[i] != NULL)
74 delete meline[i];
75
76 meline.clear();
77}
78
85void vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
86{
87 line->project(cMo);
88 p1->project(cMo);
89 p2->project(cMo);
90}
91
101void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
102{
103 vpColVector a(3);
104 vpColVector b(3);
105
106 // Calculate vector corresponding to PQ
107 a[0] = P.get_oX() - Q.get_oX();
108 a[1] = P.get_oY() - Q.get_oY();
109 a[2] = P.get_oZ() - Q.get_oZ();
110
111 // Calculate vector corresponding to PR
112 b[0] = P.get_oX() - R.get_oX();
113 b[1] = P.get_oY() - R.get_oY();
114 b[2] = P.get_oZ() - R.get_oZ();
115
116 // Calculate normal vector to plane PQ x PR
118
119 // Equation of the plane is given by:
120 double A = n[0];
121 double B = n[1];
122 double C = n[2];
123 double D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
124
125 double norm = sqrt(A * A + B * B + C * C);
126 plane.setA(A / norm);
127 plane.setB(B / norm);
128 plane.setC(C / norm);
129 plane.setD(D / norm);
130}
131
145void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
146{
147 vpPlane plane1;
148 vpPlane plane2;
149 buildPlane(P1, P2, P3, plane1);
150 buildPlane(P1, P2, P4, plane2);
151
152 L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), plane2.getA(), plane2.getB(),
153 plane2.getC(), plane2.getD());
154}
155
165{
166 if (line == NULL) {
167 line = new vpLine;
168 }
169
170 poly.setNbPoint(2);
171 poly.addPoint(0, _p1);
172 poly.addPoint(1, _p2);
173
174 p1 = &poly.p[0];
175 p2 = &poly.p[1];
176
177 vpColVector V1(3);
178 vpColVector V2(3);
179
180 V1[0] = p1->get_oX();
181 V1[1] = p1->get_oY();
182 V1[2] = p1->get_oZ();
183 V2[0] = p2->get_oX();
184 V2[1] = p2->get_oY();
185 V2[2] = p2->get_oZ();
186
187 // if((V1-V2).sumSquare()!=0)
188 if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
189 vpColVector V3(3);
190 V3[0] = double(rand_gen.next() % 1000) / 100;
191 V3[1] = double(rand_gen.next() % 1000) / 100;
192 V3[2] = double(rand_gen.next() % 1000) / 100;
193
194 vpColVector v_tmp1, v_tmp2;
195 v_tmp1 = V2 - V1;
196 v_tmp2 = V3 - V1;
197 vpColVector V4 = vpColVector::cross(v_tmp1, v_tmp2);
198
199 vpPoint P3(V3[0], V3[1], V3[2]);
200 vpPoint P4(V4[0], V4[1], V4[2]);
201 buildLine(*p1, *p2, P3, P4, *line);
202 } else {
203 vpPoint P3(V1[0], V1[1], V1[2]);
204 vpPoint P4(V2[0], V2[1], V2[2]);
205 buildLine(*p1, *p2, P3, P4, *line);
206 }
207}
208
215{
216 Lindex_polygon.push_back(idx);
217 Lindex_polygon_tracked.push_back(true);
218}
219
227void vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
228{
229 unsigned int ind = 0;
230 for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
231 if ((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) {
232 Lindex_polygon_tracked[ind] = track;
233 }
234 ind++;
235 }
236
237 isTrackedLine = false;
238 for (unsigned int i = 0; i < Lindex_polygon_tracked.size(); i++)
239 if (Lindex_polygon_tracked[i]) {
240 isTrackedLine = true;
241 break;
242 }
243
244 if (!isTrackedLine) {
245 isTrackedLineWithVisibility = false;
246 return;
247 }
248
250}
251
257{
258 if (!isTrackedLine) {
259 isTrackedLineWithVisibility = false;
260 return;
261 }
262
263 unsigned int ind = 0;
264 isTrackedLineWithVisibility = false;
265 for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
266 if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) {
267 isTrackedLineWithVisibility = true;
268 break;
269 }
270 ind++;
271 }
272}
273
280{
281 me = _me;
282
283 for (unsigned int i = 0; i < meline.size(); i++)
284 if (meline[i] != NULL) {
285 // nbFeature[i] = 0;
286 meline[i]->reset();
287 meline[i]->setMe(me);
288 }
289
290 // nbFeatureTotal = 0;
291}
292
305 const vpImage<bool> *mask)
306{
307 for (unsigned int i = 0; i < meline.size(); i++) {
308 if (meline[i] != NULL)
309 delete meline[i];
310 }
311
312 meline.clear();
313 nbFeature.clear();
314 nbFeatureTotal = 0;
315
316 if (isvisible) {
317 p1->changeFrame(cMo);
318 p2->changeFrame(cMo);
319
320 if (poly.getClipping() > 3) // Contains at least one FOV constraint
321 cam.computeFov(I.getWidth(), I.getHeight());
322
323 poly.computePolygonClipped(cam);
324
325 if (poly.polyClipped.size() == 2) { // Les points sont visibles.
326
327 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
328
329 if (useScanLine) {
330 hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
331 } else {
332 linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
333 }
334
335 if (linesLst.size() == 0) {
336 return false;
337 }
338
339 line->changeFrame(cMo);
340 try {
341 line->projection();
342 } catch (...) {
343 isvisible = false;
344 return false;
345 }
346 double rho, theta;
347 // rho theta uv
349
350 while (theta > M_PI) {
351 theta -= M_PI;
352 }
353 while (theta < -M_PI) {
354 theta += M_PI;
355 }
356
357 if (theta < -M_PI / 2.0)
358 theta = -theta - 3 * M_PI / 2.0;
359 else
360 theta = M_PI / 2.0 - theta;
361
362 for (unsigned int i = 0; i < linesLst.size(); i++) {
363 vpImagePoint ip1, ip2;
364
365 linesLst[i].first.project();
366 linesLst[i].second.project();
367
368 vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
369 vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
370
371 vpMbtMeLine *melinePt = new vpMbtMeLine;
372 melinePt->setMask(*mask);
373 melinePt->setMe(me);
374
375 melinePt->setInitRange(0);
376
377 int marge = /*10*/ 5; // ou 5 normalement
378 if (ip1.get_j() < ip2.get_j()) {
379 melinePt->jmin = (int)ip1.get_j() - marge;
380 melinePt->jmax = (int)ip2.get_j() + marge;
381 } else {
382 melinePt->jmin = (int)ip2.get_j() - marge;
383 melinePt->jmax = (int)ip1.get_j() + marge;
384 }
385 if (ip1.get_i() < ip2.get_i()) {
386 melinePt->imin = (int)ip1.get_i() - marge;
387 melinePt->imax = (int)ip2.get_i() + marge;
388 } else {
389 melinePt->imin = (int)ip2.get_i() - marge;
390 melinePt->imax = (int)ip1.get_i() + marge;
391 }
392
393 try {
394 melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
395 meline.push_back(melinePt);
396 nbFeature.push_back((unsigned int)melinePt->getMeList().size());
397 nbFeatureTotal += nbFeature.back();
398 } catch (...) {
399 delete melinePt;
400 isvisible = false;
401 return false;
402 }
403 }
404 } else {
405 isvisible = false;
406 }
407 }
408
409 return true;
410}
411
418{
419 if (isvisible) {
420 try {
421 nbFeature.clear();
422 nbFeatureTotal = 0;
423 for (size_t i = 0; i < meline.size(); i++) {
424 meline[i]->track(I);
425 nbFeature.push_back((unsigned int)meline[i]->getMeList().size());
426 nbFeatureTotal += (unsigned int)meline[i]->getMeList().size();
427 }
428 } catch (...) {
429 for (size_t i = 0; i < meline.size(); i++) {
430 if (meline[i] != NULL)
431 delete meline[i];
432 }
433
434 nbFeature.clear();
435 meline.clear();
436 nbFeatureTotal = 0;
437 Reinit = true;
438 isvisible = false;
439 }
440 }
441}
442
450{
451 if (isvisible) {
452 p1->changeFrame(cMo);
453 p2->changeFrame(cMo);
454
455 if (poly.getClipping() > 3) // Contains at least one FOV constraint
456 cam.computeFov(I.getWidth(), I.getHeight());
457
458 poly.computePolygonClipped(cam);
459
460 if (poly.polyClipped.size() == 2) { // Les points sont visibles.
461
462 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
463
464 if (useScanLine) {
465 hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
466 } else {
467 linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
468 }
469
470 if (linesLst.size() != meline.size() || linesLst.size() == 0) {
471 for (size_t i = 0; i < meline.size(); i++) {
472 if (meline[i] != NULL)
473 delete meline[i];
474 }
475
476 meline.clear();
477 nbFeature.clear();
478 nbFeatureTotal = 0;
479 isvisible = false;
480 Reinit = true;
481 } else {
482 line->changeFrame(cMo);
483 try {
484 line->projection();
485 } catch (...) {
486 for (size_t j = 0; j < meline.size(); j++) {
487 if (meline[j] != NULL)
488 delete meline[j];
489 }
490
491 meline.clear();
492 nbFeature.clear();
493 nbFeatureTotal = 0;
494 isvisible = false;
495 Reinit = true;
496 return;
497 }
498 double rho, theta;
499 // rho theta uv
501
502 while (theta > M_PI) {
503 theta -= M_PI;
504 }
505 while (theta < -M_PI) {
506 theta += M_PI;
507 }
508
509 if (theta < -M_PI / 2.0)
510 theta = -theta - 3 * M_PI / 2.0;
511 else
512 theta = M_PI / 2.0 - theta;
513
514 try {
515 for (unsigned int i = 0; i < linesLst.size(); i++) {
516 vpImagePoint ip1, ip2;
517
518 linesLst[i].first.project();
519 linesLst[i].second.project();
520
521 vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
522 vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
523
524 int marge = /*10*/ 5; // ou 5 normalement
525 if (ip1.get_j() < ip2.get_j()) {
526 meline[i]->jmin = (int)ip1.get_j() - marge;
527 meline[i]->jmax = (int)ip2.get_j() + marge;
528 } else {
529 meline[i]->jmin = (int)ip2.get_j() - marge;
530 meline[i]->jmax = (int)ip1.get_j() + marge;
531 }
532 if (ip1.get_i() < ip2.get_i()) {
533 meline[i]->imin = (int)ip1.get_i() - marge;
534 meline[i]->imax = (int)ip2.get_i() + marge;
535 } else {
536 meline[i]->imin = (int)ip2.get_i() - marge;
537 meline[i]->imax = (int)ip1.get_i() + marge;
538 }
539
540 meline[i]->updateParameters(I, ip1, ip2, rho, theta);
541 nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
543 }
544 } catch (...) {
545 for (size_t j = 0; j < meline.size(); j++) {
546 if (meline[j] != NULL)
547 delete meline[j];
548 }
549
550 meline.clear();
551 nbFeature.clear();
552 nbFeatureTotal = 0;
553 isvisible = false;
554 Reinit = true;
555 }
556 }
557 } else {
558 for (size_t i = 0; i < meline.size(); i++) {
559 if (meline[i] != NULL)
560 delete meline[i];
561 }
562 nbFeature.clear();
563 meline.clear();
564 nbFeatureTotal = 0;
565 isvisible = false;
566 }
567 }
568}
569
582 const vpImage<bool> *mask)
583{
584 for (size_t i = 0; i < meline.size(); i++) {
585 if (meline[i] != NULL)
586 delete meline[i];
587 }
588
589 nbFeature.clear();
590 meline.clear();
591 nbFeatureTotal = 0;
592
593 if (!initMovingEdge(I, cMo, false, mask))
594 Reinit = true;
595
596 Reinit = false;
597}
598
611 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
612 bool displayFullModel)
613{
614 std::vector<std::vector<double> > models =
615 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
616
617 for (size_t i = 0; i < models.size(); i++) {
618 vpImagePoint ip1(models[i][1], models[i][2]);
619 vpImagePoint ip2(models[i][3], models[i][4]);
620 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
621 }
622}
623
636 const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
637 bool displayFullModel)
638{
639 std::vector<std::vector<double> > models =
640 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
641
642 for (size_t i = 0; i < models.size(); i++) {
643 vpImagePoint ip1(models[i][1], models[i][2]);
644 vpImagePoint ip2(models[i][3], models[i][4]);
645 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
646 }
647}
648
664{
665 for (size_t i = 0; i < meline.size(); i++) {
666 if (meline[i] != NULL) {
667 meline[i]->display(I);
668 }
669 }
670}
671
673{
674 for (size_t i = 0; i < meline.size(); i++) {
675 if (meline[i] != NULL) {
676 meline[i]->display(I);
677 }
678 }
679}
680
685std::vector<std::vector<double> > vpMbtDistanceLine::getFeaturesForDisplay()
686{
687 std::vector<std::vector<double> > features;
688
689 for (size_t i = 0; i < meline.size(); i++) {
690 vpMbtMeLine *me_l = meline[i];
691 if (me_l != NULL) {
692 for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
693 vpMeSite p_me_l = *it;
694#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
695 std::vector<double> params = {0, // ME
696 p_me_l.get_ifloat(), p_me_l.get_jfloat(), static_cast<double>(p_me_l.getState())};
697#else
698 std::vector<double> params;
699 params.push_back(0); // ME
700 params.push_back(p_me_l.get_ifloat());
701 params.push_back(p_me_l.get_jfloat());
702 params.push_back(static_cast<double>(p_me_l.getState()));
703#endif
704 features.push_back(params);
705 }
706 }
707 }
708
709 return features;
710}
711
723std::vector<std::vector<double> > vpMbtDistanceLine::getModelForDisplay(unsigned int width, unsigned int height,
724 const vpHomogeneousMatrix &cMo,
725 const vpCameraParameters &camera,
726 bool displayFullModel)
727{
728 std::vector<std::vector<double> > models;
729
730 if ((isvisible && isTrackedLine) || displayFullModel) {
731 p1->changeFrame(cMo);
732 p2->changeFrame(cMo);
733
734 vpImagePoint ip1, ip2;
735 vpCameraParameters c = camera;
736 if (poly.getClipping() > 3) // Contains at least one FOV constraint
737 c.computeFov(width, height);
738
739 poly.computePolygonClipped(c);
740
741 if (poly.polyClipped.size() == 2 &&
742 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
743 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
744 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
745 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
746 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
747 ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
748
749 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
750 if (useScanLine && !displayFullModel) {
751 hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
752 } else {
753 linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
754 }
755
756 for (unsigned int i = 0; i < linesLst.size(); i++) {
757 linesLst[i].first.project();
758 linesLst[i].second.project();
759
760 vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
761 vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
762
763#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
764 std::vector<double> params = {0, // 0 for line parameters
765 ip1.get_i(), ip1.get_j(), ip2.get_i(), ip2.get_j()};
766#else
767 std::vector<double> params;
768 params.push_back(0); // 0 for line parameters
769 params.push_back(ip1.get_i());
770 params.push_back(ip1.get_j());
771 params.push_back(ip2.get_i());
772 params.push_back(ip2.get_j());
773#endif
774 models.push_back(params);
775 }
776 }
777 }
778
779 return models;
780}
781
786{
787 if (isvisible) {
790 } else {
791 for (size_t i = 0; i < meline.size(); i++) {
792 nbFeature[i] = 0;
793 // To be consistent with nbFeature[i] = 0
794 std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
795 me_site_list.clear();
796 }
797 nbFeatureTotal = 0;
798 }
799}
800
806{
807 if (isvisible) {
808 try {
809 // feature projection
810 line->changeFrame(cMo);
811 line->projection();
812
813 vpFeatureBuilder::create(featureline, *line);
814
815 double rho = featureline.getRho();
816 double theta = featureline.getTheta();
817
818 double co = cos(theta);
819 double si = sin(theta);
820
821 double mx = 1.0 / cam.get_px();
822 double my = 1.0 / cam.get_py();
823 double xc = cam.get_u0();
824 double yc = cam.get_v0();
825
826 double alpha_;
827 vpMatrix H = featureline.interaction();
828
829 double x, y;
830 unsigned int j = 0;
831
832 for (size_t i = 0; i < meline.size(); i++) {
833 for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
834 it != meline[i]->getMeList().end(); ++it) {
835 x = (double)it->j;
836 y = (double)it->i;
837
838 x = (x - xc) * mx;
839 y = (y - yc) * my;
840
841 alpha_ = x * si - y * co;
842
843 double *Lrho = H[0];
844 double *Ltheta = H[1];
845 // Calculate interaction matrix for a distance
846 for (unsigned int k = 0; k < 6; k++) {
847 L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
848 }
849 error[j] = rho - (x * co + y * si);
850 j++;
851 }
852 }
853 } catch (...) {
854 // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
855 // Set the corresponding interaction matrix part to zero
856 unsigned int j = 0;
857 for (size_t i = 0; i < meline.size(); i++) {
858 for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
859 it != meline[i]->getMeList().end(); ++it) {
860 for (unsigned int k = 0; k < 6; k++) {
861 L[j][k] = 0.0;
862 }
863
864 error[j] = 0.0;
865 j++;
866 }
867 }
868 }
869 }
870}
871
880bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
881{
882 if (threshold > I.getWidth() || threshold > I.getHeight()) {
883 return true;
884 }
885 if (isvisible) {
886
887 for (size_t i = 0; i < meline.size(); i++) {
888 for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
889 ++it) {
890 int i_ = it->i;
891 int j_ = it->j;
892
893 if (i_ < 0 || j_ < 0) { // out of image.
894 return true;
895 }
896
897 if (((unsigned int)i_ > (I.getHeight() - threshold)) || (unsigned int)i_ < threshold ||
898 ((unsigned int)j_ > (I.getWidth() - threshold)) || (unsigned int)j_ < threshold) {
899 return true;
900 }
901 }
902 }
903 }
904 return false;
905}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
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 ...
double get_j() const
double get_i() const
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
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition vpLine.cpp:327
double getRho() const
Definition vpLine.h:129
void projection()
Definition vpLine.cpp:190
double getTheta() const
Definition vpLine.h:141
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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 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.
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:65
vpMeSiteState getState() const
Definition vpMeSite.h:261
double get_ifloat() const
Definition vpMeSite.h:231
double get_jfloat() const
Definition vpMeSite.h:238
Definition vpMe.h:122
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:54
void setA(double a)
Definition vpPlane.h:80
void setD(double d)
Definition vpPlane.h:86
double getD() const
Definition vpPlane.h:106
void setC(double c)
Definition vpPlane.h:84
double getA() const
Definition vpPlane.h:100
double getC() const
Definition vpPlane.h:104
double getB() const
Definition vpPlane.h:102
void setB(double b)
Definition vpPlane.h:82
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition vpPoint.cpp:236
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
virtual void setNbPoint(unsigned int nb)
unsigned int getClipping() const
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition vpPolygon3D.h:78
void addPoint(unsigned int n, const vpPoint &P)
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122
uint32_t next()