Visual Servoing Platform  version 3.5.1 under development (2023-06-02)
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 
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 
217 void vpMbtDistanceLine::addPolygon(const int &idx)
218 {
219  Lindex_polygon.push_back(idx);
220  Lindex_polygon_tracked.push_back(true);
221 }
222 
230 void 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 
252  updateTracked();
253 }
254 
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 
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  } catch (...) {
346  isvisible = false;
347  return false;
348  }
349  double rho, theta;
350  // rho theta uv
352 
353  while (theta > M_PI) {
354  theta -= M_PI;
355  }
356  while (theta < -M_PI) {
357  theta += M_PI;
358  }
359 
360  if (theta < -M_PI / 2.0)
361  theta = -theta - 3 * M_PI / 2.0;
362  else
363  theta = M_PI / 2.0 - theta;
364 
365  for (unsigned int i = 0; i < linesLst.size(); i++) {
366  vpImagePoint ip1, ip2;
367 
368  linesLst[i].first.project();
369  linesLst[i].second.project();
370 
371  vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
372  vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
373 
374  vpMbtMeLine *melinePt = new vpMbtMeLine;
375  melinePt->setMask(*mask);
376  melinePt->setMe(me);
377 
378  melinePt->setInitRange(0);
379 
380  int marge = /*10*/ 5; // ou 5 normalement
381  if (ip1.get_j() < ip2.get_j()) {
382  melinePt->jmin = (int)ip1.get_j() - marge;
383  melinePt->jmax = (int)ip2.get_j() + marge;
384  } else {
385  melinePt->jmin = (int)ip2.get_j() - marge;
386  melinePt->jmax = (int)ip1.get_j() + marge;
387  }
388  if (ip1.get_i() < ip2.get_i()) {
389  melinePt->imin = (int)ip1.get_i() - marge;
390  melinePt->imax = (int)ip2.get_i() + marge;
391  } else {
392  melinePt->imin = (int)ip2.get_i() - marge;
393  melinePt->imax = (int)ip1.get_i() + marge;
394  }
395 
396  try {
397  melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
398  meline.push_back(melinePt);
399  nbFeature.push_back((unsigned int)melinePt->getMeList().size());
400  nbFeatureTotal += nbFeature.back();
401  } catch (...) {
402  delete melinePt;
403  isvisible = false;
404  return false;
405  }
406  }
407  } else {
408  isvisible = false;
409  }
410  }
411 
412  return true;
413 }
414 
421 {
422  if (isvisible) {
423  try {
424  nbFeature.clear();
425  nbFeatureTotal = 0;
426  for (size_t i = 0; i < meline.size(); i++) {
427  meline[i]->track(I);
428  nbFeature.push_back((unsigned int)meline[i]->getMeList().size());
429  nbFeatureTotal += (unsigned int)meline[i]->getMeList().size();
430  }
431  } catch (...) {
432  for (size_t i = 0; i < meline.size(); i++) {
433  if (meline[i] != NULL)
434  delete meline[i];
435  }
436 
437  nbFeature.clear();
438  meline.clear();
439  nbFeatureTotal = 0;
440  Reinit = true;
441  isvisible = false;
442  }
443  }
444 }
445 
453 {
454  if (isvisible) {
455  p1->changeFrame(cMo);
456  p2->changeFrame(cMo);
457 
458  if (poly.getClipping() > 3) // Contains at least one FOV constraint
459  cam.computeFov(I.getWidth(), I.getHeight());
460 
461  poly.computePolygonClipped(cam);
462 
463  if (poly.polyClipped.size() == 2) { // Les points sont visibles.
464 
465  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
466 
467  if (useScanLine) {
468  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
469  } else {
470  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
471  }
472 
473  if (linesLst.size() != meline.size() || linesLst.size() == 0) {
474  for (size_t i = 0; i < meline.size(); i++) {
475  if (meline[i] != NULL)
476  delete meline[i];
477  }
478 
479  meline.clear();
480  nbFeature.clear();
481  nbFeatureTotal = 0;
482  isvisible = false;
483  Reinit = true;
484  } else {
485  line->changeFrame(cMo);
486  try {
487  line->projection();
488  } catch (...) {
489  for (size_t j = 0; j < meline.size(); j++) {
490  if (meline[j] != NULL)
491  delete meline[j];
492  }
493 
494  meline.clear();
495  nbFeature.clear();
496  nbFeatureTotal = 0;
497  isvisible = false;
498  Reinit = true;
499  return;
500  }
501  double rho, theta;
502  // rho theta uv
504 
505  while (theta > M_PI) {
506  theta -= M_PI;
507  }
508  while (theta < -M_PI) {
509  theta += M_PI;
510  }
511 
512  if (theta < -M_PI / 2.0)
513  theta = -theta - 3 * M_PI / 2.0;
514  else
515  theta = M_PI / 2.0 - theta;
516 
517  try {
518  for (unsigned int i = 0; i < linesLst.size(); i++) {
519  vpImagePoint ip1, ip2;
520 
521  linesLst[i].first.project();
522  linesLst[i].second.project();
523 
524  vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
525  vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
526 
527  int marge = /*10*/ 5; // ou 5 normalement
528  if (ip1.get_j() < ip2.get_j()) {
529  meline[i]->jmin = (int)ip1.get_j() - marge;
530  meline[i]->jmax = (int)ip2.get_j() + marge;
531  } else {
532  meline[i]->jmin = (int)ip2.get_j() - marge;
533  meline[i]->jmax = (int)ip1.get_j() + marge;
534  }
535  if (ip1.get_i() < ip2.get_i()) {
536  meline[i]->imin = (int)ip1.get_i() - marge;
537  meline[i]->imax = (int)ip2.get_i() + marge;
538  } else {
539  meline[i]->imin = (int)ip2.get_i() - marge;
540  meline[i]->imax = (int)ip1.get_i() + marge;
541  }
542 
543  meline[i]->updateParameters(I, ip1, ip2, rho, theta);
544  nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
546  }
547  } catch (...) {
548  for (size_t j = 0; j < meline.size(); j++) {
549  if (meline[j] != NULL)
550  delete meline[j];
551  }
552 
553  meline.clear();
554  nbFeature.clear();
555  nbFeatureTotal = 0;
556  isvisible = false;
557  Reinit = true;
558  }
559  }
560  } else {
561  for (size_t i = 0; i < meline.size(); i++) {
562  if (meline[i] != NULL)
563  delete meline[i];
564  }
565  nbFeature.clear();
566  meline.clear();
567  nbFeatureTotal = 0;
568  isvisible = false;
569  }
570  }
571 }
572 
585  const vpImage<bool> *mask)
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, unsigned int thickness,
615  bool displayFullModel)
616 {
617  std::vector<std::vector<double> > models =
618  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
619 
620  for (size_t i = 0; i < models.size(); i++) {
621  vpImagePoint ip1(models[i][1], models[i][2]);
622  vpImagePoint ip2(models[i][3], models[i][4]);
623  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
624  }
625 }
626 
639  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
640  bool displayFullModel)
641 {
642  std::vector<std::vector<double> > models =
643  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
644 
645  for (size_t i = 0; i < models.size(); i++) {
646  vpImagePoint ip1(models[i][1], models[i][2]);
647  vpImagePoint ip2(models[i][3], models[i][4]);
648  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
649  }
650 }
651 
667 {
668  for (size_t i = 0; i < meline.size(); i++) {
669  if (meline[i] != NULL) {
670  meline[i]->display(I);
671  }
672  }
673 }
674 
676 {
677  for (size_t i = 0; i < meline.size(); i++) {
678  if (meline[i] != NULL) {
679  meline[i]->display(I);
680  }
681  }
682 }
683 
688 std::vector<std::vector<double> > vpMbtDistanceLine::getFeaturesForDisplay()
689 {
690  std::vector<std::vector<double> > features;
691 
692  for (size_t i = 0; i < meline.size(); i++) {
693  vpMbtMeLine *me_l = meline[i];
694  if (me_l != NULL) {
695  for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
696  vpMeSite p_me_l = *it;
697 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
698  std::vector<double> params = {0, // ME
699  p_me_l.get_ifloat(), p_me_l.get_jfloat(), static_cast<double>(p_me_l.getState())};
700 #else
701  std::vector<double> params;
702  params.push_back(0); // ME
703  params.push_back(p_me_l.get_ifloat());
704  params.push_back(p_me_l.get_jfloat());
705  params.push_back(static_cast<double>(p_me_l.getState()));
706 #endif
707  features.push_back(params);
708  }
709  }
710  }
711 
712  return features;
713 }
714 
726 std::vector<std::vector<double> > vpMbtDistanceLine::getModelForDisplay(unsigned int width, unsigned int height,
727  const vpHomogeneousMatrix &cMo,
728  const vpCameraParameters &camera,
729  bool displayFullModel)
730 {
731  std::vector<std::vector<double> > models;
732 
733  if ((isvisible && isTrackedLine) || displayFullModel) {
734  p1->changeFrame(cMo);
735  p2->changeFrame(cMo);
736 
737  vpImagePoint ip1, ip2;
738  vpCameraParameters c = camera;
739  if (poly.getClipping() > 3) // Contains at least one FOV constraint
740  c.computeFov(width, height);
741 
742  poly.computePolygonClipped(c);
743 
744  if (poly.polyClipped.size() == 2 &&
745  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
746  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
747  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
748  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
749  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
750  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
751 
752  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
753  if (useScanLine && !displayFullModel) {
754  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
755  } else {
756  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
757  }
758 
759  for (unsigned int i = 0; i < linesLst.size(); i++) {
760  linesLst[i].first.project();
761  linesLst[i].second.project();
762 
763  vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
764  vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
765 
766 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
767  std::vector<double> params = {0, // 0 for line parameters
768  ip1.get_i(), ip1.get_j(), ip2.get_i(), ip2.get_j()};
769 #else
770  std::vector<double> params;
771  params.push_back(0); // 0 for line parameters
772  params.push_back(ip1.get_i());
773  params.push_back(ip1.get_j());
774  params.push_back(ip2.get_i());
775  params.push_back(ip2.get_j());
776 #endif
777  models.push_back(params);
778  }
779  }
780  }
781 
782  return models;
783 }
784 
789 {
790  if (isvisible) {
791  L.resize(nbFeatureTotal, 6);
793  } else {
794  for (size_t i = 0; i < meline.size(); i++) {
795  nbFeature[i] = 0;
796  // To be consistent with nbFeature[i] = 0
797  std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
798  me_site_list.clear();
799  }
800  nbFeatureTotal = 0;
801  }
802 }
803 
809 {
810  if (isvisible) {
811  try {
812  // feature projection
813  line->changeFrame(cMo);
814  line->projection();
815 
816  vpFeatureBuilder::create(featureline, *line);
817 
818  double rho = featureline.getRho();
819  double theta = featureline.getTheta();
820 
821  double co = cos(theta);
822  double si = sin(theta);
823 
824  double mx = 1.0 / cam.get_px();
825  double my = 1.0 / cam.get_py();
826  double xc = cam.get_u0();
827  double yc = cam.get_v0();
828 
829  double alpha_;
830  vpMatrix H = featureline.interaction();
831 
832  double x, y;
833  unsigned int j = 0;
834 
835  for (size_t i = 0; i < meline.size(); i++) {
836  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
837  it != meline[i]->getMeList().end(); ++it) {
838  x = (double)it->j;
839  y = (double)it->i;
840 
841  x = (x - xc) * mx;
842  y = (y - yc) * my;
843 
844  alpha_ = x * si - y * co;
845 
846  double *Lrho = H[0];
847  double *Ltheta = H[1];
848  // Calculate interaction matrix for a distance
849  for (unsigned int k = 0; k < 6; k++) {
850  L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
851  }
852  error[j] = rho - (x * co + y * si);
853  j++;
854  }
855  }
856  } catch (...) {
857  // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
858  // Set the corresponding interaction matrix part to zero
859  unsigned int j = 0;
860  for (size_t i = 0; i < meline.size(); i++) {
861  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
862  it != meline[i]->getMeList().end(); ++it) {
863  for (unsigned int k = 0; k < 6; k++) {
864  L[j][k] = 0.0;
865  }
866 
867  error[j] = 0.0;
868  j++;
869  }
870  }
871  }
872  }
873 }
874 
883 bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
884 {
885  if (threshold > I.getWidth() || threshold > I.getHeight()) {
886  return true;
887  }
888  if (isvisible) {
889 
890  for (size_t i = 0; i < meline.size(); i++) {
891  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
892  ++it) {
893  int i_ = it->i;
894  int j_ = it->j;
895 
896  if (i_ < 0 || j_ < 0) { // out of image.
897  return true;
898  }
899 
900  if (((unsigned int)i_ > (I.getHeight() - threshold)) || (unsigned int)i_ < threshold ||
901  ((unsigned int)j_ > (I.getWidth() - threshold)) || (unsigned int)j_ < threshold) {
902  return true;
903  }
904  }
905  }
906  }
907  return false;
908 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:307
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:172
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:397
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
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:89
double get_j() const
Definition: vpImagePoint.h:132
double get_i() const
Definition: vpImagePoint.h:121
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
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:67
vpMeSiteState getState() const
Definition: vpMeSite.h:189
double get_ifloat() const
Definition: vpMeSite.h:159
double get_jfloat() const
Definition: vpMeSite.h:166
Definition: vpMe.h:119
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:85
void setD(double d)
Definition: vpPlane.h:91
double getD() const
Definition: vpPlane.h:111
void setC(double c)
Definition: vpPlane.h:89
double getA() const
Definition: vpPlane.h:105
double getC() const
Definition: vpPlane.h:109
double getB() const
Definition: vpPlane.h:107
void setB(double b)
Definition: vpPlane.h:87
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:124
uint32_t next()
Definition: vpUniRand.cpp:146