Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
53 void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
54 void 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 
88 void vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
89 {
90  line->project(cMo);
91  p1->project(cMo);
92  p2->project(cMo);
93 }
94 
104 void 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 
148 void 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 
167 {
168  if (line == NULL) {
169  line = new vpLine;
170  }
171 
172  poly.setNbPoint(2);
173  poly.addPoint(0, _p1);
174  poly.addPoint(1, _p2);
175 
176  p1 = &poly.p[0];
177  p2 = &poly.p[1];
178 
179  vpColVector V1(3);
180  vpColVector V2(3);
181 
182  V1[0] = p1->get_oX();
183  V1[1] = p1->get_oY();
184  V1[2] = p1->get_oZ();
185  V2[0] = p2->get_oX();
186  V2[1] = p2->get_oY();
187  V2[2] = p2->get_oZ();
188 
189  // if((V1-V2).sumSquare()!=0)
190  if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
191  vpColVector V3(3);
192  V3[0] = double(rand() % 1000) / 100;
193  V3[1] = double(rand() % 1000) / 100;
194  V3[2] = double(rand() % 1000) / 100;
195 
196  vpColVector v_tmp1, v_tmp2;
197  v_tmp1 = V2 - V1;
198  v_tmp2 = V3 - V1;
199  vpColVector V4 = vpColVector::cross(v_tmp1, v_tmp2);
200 
201  vpPoint P3(V3[0], V3[1], V3[2]);
202  vpPoint P4(V4[0], V4[1], V4[2]);
203  buildLine(*p1, *p2, P3, P4, *line);
204  } else {
205  vpPoint P3(V1[0], V1[1], V1[2]);
206  vpPoint P4(V2[0], V2[1], V2[2]);
207  buildLine(*p1, *p2, P3, P4, *line);
208  }
209 }
210 
216 void vpMbtDistanceLine::addPolygon(const int &idx)
217 {
218  Lindex_polygon.push_back(idx);
219  Lindex_polygon_tracked.push_back(true);
220 }
221 
229 void vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
230 {
231  unsigned int ind = 0;
232  for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
233  if ((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) {
234  Lindex_polygon_tracked[ind] = track;
235  }
236  ind++;
237  }
238 
239  isTrackedLine = false;
240  for (unsigned int i = 0; i < Lindex_polygon_tracked.size(); i++)
241  if (Lindex_polygon_tracked[i]) {
242  isTrackedLine = true;
243  break;
244  }
245 
246  if (!isTrackedLine) {
247  isTrackedLineWithVisibility = false;
248  return;
249  }
250 
251  updateTracked();
252 }
253 
260 {
261  if (!isTrackedLine) {
262  isTrackedLineWithVisibility = false;
263  return;
264  }
265 
266  unsigned int ind = 0;
267  isTrackedLineWithVisibility = false;
268  for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
269  if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) {
270  isTrackedLineWithVisibility = true;
271  break;
272  }
273  ind++;
274  }
275 }
276 
283 {
284  me = _me;
285 
286  for (unsigned int i = 0; i < meline.size(); i++)
287  if (meline[i] != NULL) {
288  // nbFeature[i] = 0;
289  meline[i]->reset();
290  meline[i]->setMe(me);
291  }
292 
293  // nbFeatureTotal = 0;
294 }
295 
307 bool vpMbtDistanceLine::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack,
308  const vpImage<bool> *mask)
309 {
310  for (unsigned int i = 0; i < meline.size(); i++) {
311  if (meline[i] != NULL)
312  delete meline[i];
313  }
314 
315  meline.clear();
316  nbFeature.clear();
317  nbFeatureTotal = 0;
318 
319  if (isvisible) {
320  p1->changeFrame(cMo);
321  p2->changeFrame(cMo);
322 
323  if (poly.getClipping() > 3) // Contains at least one FOV constraint
324  cam.computeFov(I.getWidth(), I.getHeight());
325 
326  poly.computePolygonClipped(cam);
327 
328  if (poly.polyClipped.size() == 2) { // Les points sont visibles.
329 
330  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
331 
332  if (useScanLine) {
333  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
334  } else {
335  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
336  }
337 
338  if (linesLst.size() == 0) {
339  return false;
340  }
341 
342  line->changeFrame(cMo);
343  try {
344  line->projection();
345  }
346  catch(const vpException &e) {
347  isvisible = false;
348  return false;
349  }
350  double rho, theta;
351  // rho theta uv
353 
354  while (theta > M_PI) {
355  theta -= M_PI;
356  }
357  while (theta < -M_PI) {
358  theta += M_PI;
359  }
360 
361  if (theta < -M_PI / 2.0)
362  theta = -theta - 3 * M_PI / 2.0;
363  else
364  theta = M_PI / 2.0 - theta;
365 
366  for (unsigned int i = 0; i < linesLst.size(); i++) {
367  vpImagePoint ip1, ip2;
368 
369  linesLst[i].first.project();
370  linesLst[i].second.project();
371 
372  vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
373  vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
374 
375  vpMbtMeLine *melinePt = new vpMbtMeLine;
376  melinePt->setMask(*mask);
377  melinePt->setMe(me);
378 
379  melinePt->setInitRange(0);
380 
381  int marge = /*10*/ 5; // ou 5 normalement
382  if (ip1.get_j() < ip2.get_j()) {
383  melinePt->jmin = (int)ip1.get_j() - marge;
384  melinePt->jmax = (int)ip2.get_j() + marge;
385  } else {
386  melinePt->jmin = (int)ip2.get_j() - marge;
387  melinePt->jmax = (int)ip1.get_j() + marge;
388  }
389  if (ip1.get_i() < ip2.get_i()) {
390  melinePt->imin = (int)ip1.get_i() - marge;
391  melinePt->imax = (int)ip2.get_i() + marge;
392  } else {
393  melinePt->imin = (int)ip2.get_i() - marge;
394  melinePt->imax = (int)ip1.get_i() + marge;
395  }
396 
397  try {
398  melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
399  meline.push_back(melinePt);
400  nbFeature.push_back((unsigned int) melinePt->getMeList().size());
401  nbFeatureTotal += nbFeature.back();
402  } catch (...) {
403  delete melinePt;
404  isvisible = false;
405  return false;
406  }
407  }
408  } else {
409  isvisible = false;
410  }
411  }
412 
413  return true;
414 }
415 
422 {
423  if (isvisible) {
424  try {
425  nbFeature.clear();
426  nbFeatureTotal = 0;
427  for (size_t i = 0; i < meline.size(); i++) {
428  meline[i]->track(I);
429  nbFeature.push_back((unsigned int)meline[i]->getMeList().size());
430  nbFeatureTotal += (unsigned int)meline[i]->getMeList().size();
431  }
432  } catch (...) {
433  for (size_t i = 0; i < meline.size(); i++) {
434  if (meline[i] != NULL)
435  delete meline[i];
436  }
437 
438  nbFeature.clear();
439  meline.clear();
440  nbFeatureTotal = 0;
441  Reinit = true;
442  isvisible = false;
443  }
444  }
445 }
446 
454 {
455  if (isvisible) {
456  p1->changeFrame(cMo);
457  p2->changeFrame(cMo);
458 
459  if (poly.getClipping() > 3) // Contains at least one FOV constraint
460  cam.computeFov(I.getWidth(), I.getHeight());
461 
462  poly.computePolygonClipped(cam);
463 
464  if (poly.polyClipped.size() == 2) { // Les points sont visibles.
465 
466  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
467 
468  if (useScanLine) {
469  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
470  } else {
471  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
472  }
473 
474  if (linesLst.size() != meline.size() || linesLst.size() == 0) {
475  for (size_t i = 0; i < meline.size(); i++) {
476  if (meline[i] != NULL)
477  delete meline[i];
478  }
479 
480  meline.clear();
481  nbFeature.clear();
482  nbFeatureTotal = 0;
483  isvisible = false;
484  Reinit = true;
485  } else {
486  line->changeFrame(cMo);
487  try {
488  line->projection();
489  }
490  catch(const vpException &e) {
491  for (size_t j = 0; j < meline.size(); j++) {
492  if (meline[j] != NULL)
493  delete meline[j];
494  }
495 
496  meline.clear();
497  nbFeature.clear();
498  nbFeatureTotal = 0;
499  isvisible = false;
500  Reinit = true;
501  return;
502  }
503  double rho, theta;
504  // rho theta uv
506 
507  while (theta > M_PI) {
508  theta -= M_PI;
509  }
510  while (theta < -M_PI) {
511  theta += M_PI;
512  }
513 
514  if (theta < -M_PI / 2.0)
515  theta = -theta - 3 * M_PI / 2.0;
516  else
517  theta = M_PI / 2.0 - theta;
518 
519  try {
520  for (unsigned int i = 0; i < linesLst.size(); i++) {
521  vpImagePoint ip1, ip2;
522 
523  linesLst[i].first.project();
524  linesLst[i].second.project();
525 
526  vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
527  vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
528 
529  int marge = /*10*/ 5; // ou 5 normalement
530  if (ip1.get_j() < ip2.get_j()) {
531  meline[i]->jmin = (int)ip1.get_j() - marge;
532  meline[i]->jmax = (int)ip2.get_j() + marge;
533  } else {
534  meline[i]->jmin = (int)ip2.get_j() - marge;
535  meline[i]->jmax = (int)ip1.get_j() + marge;
536  }
537  if (ip1.get_i() < ip2.get_i()) {
538  meline[i]->imin = (int)ip1.get_i() - marge;
539  meline[i]->imax = (int)ip2.get_i() + marge;
540  } else {
541  meline[i]->imin = (int)ip2.get_i() - marge;
542  meline[i]->imax = (int)ip1.get_i() + marge;
543  }
544 
545  meline[i]->updateParameters(I, ip1, ip2, rho, theta);
546  nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
548  }
549  } catch (...) {
550  for (size_t j = 0; j < meline.size(); j++) {
551  if (meline[j] != NULL)
552  delete meline[j];
553  }
554 
555  meline.clear();
556  nbFeature.clear();
557  nbFeatureTotal = 0;
558  isvisible = false;
559  Reinit = true;
560  }
561  }
562  } else {
563  for (size_t i = 0; i < meline.size(); i++) {
564  if (meline[i] != NULL)
565  delete meline[i];
566  }
567  nbFeature.clear();
568  meline.clear();
569  nbFeatureTotal = 0;
570  isvisible = false;
571  }
572  }
573 }
574 
586 {
587  for (size_t i = 0; i < meline.size(); i++) {
588  if (meline[i] != NULL)
589  delete meline[i];
590  }
591 
592  nbFeature.clear();
593  meline.clear();
594  nbFeatureTotal = 0;
595 
596  if (!initMovingEdge(I, cMo, false, mask))
597  Reinit = true;
598 
599  Reinit = false;
600 }
601 
614  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
615  const bool displayFullModel)
616 {
617  if ((isvisible && isTrackedLine) || displayFullModel) {
618  p1->changeFrame(cMo);
619  p2->changeFrame(cMo);
620 
621  vpImagePoint ip1, ip2;
622  vpCameraParameters c = camera;
623  if (poly.getClipping() > 3) // Contains at least one FOV constraint
624  c.computeFov(I.getWidth(), I.getHeight());
625 
626  poly.computePolygonClipped(c);
627 
628  if (poly.polyClipped.size() == 2 &&
629  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
630  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
631  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
632  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
633  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
634  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
635 
636  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
637  if (useScanLine && !displayFullModel) {
638  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
639  } else {
640  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
641  }
642 
643  for (unsigned int i = 0; i < linesLst.size(); i++) {
644  linesLst[i].first.project();
645  linesLst[i].second.project();
646 
647  vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
648  vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
649 
650  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
651  }
652  }
653  }
654 }
655 
668  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
669  const bool displayFullModel)
670 {
671  if ((isvisible && isTrackedLine) || displayFullModel) {
672  p1->changeFrame(cMo);
673  p2->changeFrame(cMo);
674 
675  vpImagePoint ip1, ip2;
676  vpCameraParameters c = camera;
677  if (poly.getClipping() > 3) // Contains at least one FOV constraint
678  c.computeFov(I.getWidth(), I.getHeight());
679 
680  poly.computePolygonClipped(c);
681 
682  if (poly.polyClipped.size() == 2 &&
683  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
684  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
685  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
686  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
687  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
688  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
689 
690  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
691  if (useScanLine && !displayFullModel) {
692  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
693  } else {
694  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
695  }
696 
697  for (unsigned int i = 0; i < linesLst.size(); i++) {
698  linesLst[i].first.project();
699  linesLst[i].second.project();
700 
701  vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
702  vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
703 
704  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
705  }
706  }
707  }
708 }
709 
725 {
726  for (size_t i = 0; i < meline.size(); i++)
727  if (meline[i] != NULL) {
728  meline[i]->display(I);
729  }
730 }
731 
736 {
737  if (isvisible) {
738  L.resize(nbFeatureTotal, 6);
740  } else {
741  for (size_t i = 0; i < meline.size(); i++) {
742  nbFeature[i] = 0;
743  // To be consistent with nbFeature[i] = 0
744  std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
745  me_site_list.clear();
746  }
747  nbFeatureTotal = 0;
748  }
749 }
750 
756 {
757  if (isvisible) {
758  try {
759  // feature projection
760  line->changeFrame(cMo);
761  line->projection();
762 
763  vpFeatureBuilder::create(featureline, *line);
764 
765  double rho = featureline.getRho();
766  double theta = featureline.getTheta();
767 
768  double co = cos(theta);
769  double si = sin(theta);
770 
771  double mx = 1.0 / cam.get_px();
772  double my = 1.0 / cam.get_py();
773  double xc = cam.get_u0();
774  double yc = cam.get_v0();
775 
776  double alpha_;
777  vpMatrix H = featureline.interaction();
778 
779  double x, y;
780  unsigned int j = 0;
781 
782  for (size_t i = 0; i < meline.size(); i++) {
783  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
784  it != meline[i]->getMeList().end(); ++it) {
785  x = (double)it->j;
786  y = (double)it->i;
787 
788  x = (x - xc) * mx;
789  y = (y - yc) * my;
790 
791  alpha_ = x * si - y * co;
792 
793  double *Lrho = H[0];
794  double *Ltheta = H[1];
795  // Calculate interaction matrix for a distance
796  for (unsigned int k = 0; k < 6; k++) {
797  L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
798  }
799  error[j] = rho - (x * co + y * si);
800  j++;
801  }
802  }
803  } catch (const vpException &e) {
804  // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
805  // Set the corresponding interaction matrix part to zero
806  unsigned int j = 0;
807  for (size_t i = 0; i < meline.size(); i++) {
808  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
809  it != meline[i]->getMeList().end(); ++it) {
810  for (unsigned int k = 0; k < 6; k++) {
811  L[j][k] = 0.0;
812  }
813 
814  error[j] = 0.0;
815  j++;
816  }
817  }
818  }
819  }
820 }
821 
830 bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
831 {
832  if (threshold > I.getWidth() || threshold > I.getHeight()) {
833  return true;
834  }
835  if (isvisible) {
836 
837  for (size_t i = 0; i < meline.size(); i++) {
838  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
839  ++it) {
840  int i_ = it->i;
841  int j_ = it->j;
842 
843  if (i_ < 0 || j_ < 0) { // out of image.
844  return true;
845  }
846 
847  if (((unsigned int)i_ > (I.getHeight() - threshold)) || (unsigned int)i_ < threshold ||
848  ((unsigned int)j_ > (I.getWidth() - threshold)) || (unsigned int)j_ < threshold) {
849  return true;
850  }
851  }
852  }
853  }
854  return false;
855 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void setD(const double d)
Definition: vpPlane.h:88
void trackMovingEdge(const vpImage< unsigned char > &I)
double get_u0() const
virtual void setNbPoint(const unsigned int nb)
unsigned int nbFeatureTotal
The number of moving edges.
vpLine * line
The 3D line.
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double get_i() const
Definition: vpImagePoint.h:204
unsigned int getWidth() const
Definition: vpImage.h:239
bool Reinit
Indicates if the line has to be reinitialized.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:285
Implementation of an homogeneous matrix and operations on such kind of matrices.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Definition: vpLine.cpp:85
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
std::vector< bool > Lindex_polygon_tracked
vpPoint * p1
The first extremity.
void displayMovingEdges(const vpImage< unsigned char > &I)
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:422
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
Definition: vpMe.h:60
double getRho() const
double get_py() const
double getTheta() const
bool isvisible
Indicates if the line is visible or not.
double get_j() const
Definition: vpImagePoint.h:215
Class that defines what is a point.
Definition: vpPoint.h:58
double getTheta() const
Definition: vpLine.h:146
double getRho() const
Definition: vpLine.h:157
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition: vpPolygon3D.h:83
void setTracked(const std::string &name, const bool &track)
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:105
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
double get_v0() const
vpPoint * p2
The second extremity.
vpColVector error
The error vector.
void addPoint(const unsigned int n, const vpPoint &P)
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:424
unsigned int getClipping() const
Definition: vpPolygon3D.h:118
void setA(const double a)
Definition: vpPlane.h:82
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
void projection()
Definition: vpLine.cpp:193
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpLine.cpp:336
double get_px() const
void setC(const double c)
Definition: vpPlane.h:86
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:420
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double getB() const
Definition: vpPlane.h:104
void setB(const double b)
Definition: vpPlane.h:84
double getA() const
Definition: vpPlane.h:102
void addPolygon(const int &index)
unsigned int getHeight() const
Definition: vpImage.h:178
double getC() const
Definition: vpPlane.h:106
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
std::vector< vpMbtMeLine * > meline
The moving edge container.
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:233
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
vpMatrix L
The interaction matrix.
std::vector< unsigned int > nbFeature
The number of moving edges.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
double getD() const
Definition: vpPlane.h:108
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
void computeFov(const unsigned int &w, const unsigned int &h)