Visual Servoing Platform  version 3.4.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 
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 
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 
689 std::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 
729 std::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) {
797  L.resize(nbFeatureTotal, 6);
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 
889 bool 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 addPoint(unsigned int n, const vpPoint &P)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void trackMovingEdge(const vpImage< unsigned char > &I)
double get_u0() const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
unsigned int nbFeatureTotal
The number of moving edges.
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
uint32_t next()
Definition: vpUniRand.cpp:149
vpLine * line
The 3D line.
double get_jfloat() const
Definition: vpMeSite.h:167
void setD(double d)
Definition: vpPlane.h:88
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double get_i() const
Definition: vpImagePoint.h:203
unsigned int getWidth() const
Definition: vpImage.h:246
bool Reinit
Indicates if the line has to be reinitialized.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:351
void setC(double c)
Definition: vpPlane.h:86
Implementation of an homogeneous matrix and operations on such kind of matrices.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
virtual void setNbPoint(unsigned int nb)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
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 oY coordinate in the object frame.
Definition: vpPoint.cpp:449
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:214
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
vpMeSiteState getState() const
Definition: vpMeSite.h:190
double getTheta() const
Definition: vpLine.h:142
void setA(double a)
Definition: vpPlane.h:82
double getRho() const
Definition: vpLine.h:130
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 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:104
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition: vpLine.cpp:85
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
double get_v0() const
vpPoint * p2
The second extremity.
vpColVector error
The error vector.
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:451
unsigned int getClipping() const
Definition: vpPolygon3D.h:118
void projection()
Definition: vpLine.cpp:193
double get_px() const
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 oX coordinate in the object frame.
Definition: vpPoint.cpp:447
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:130
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
double get_ifloat() const
Definition: vpMeSite.h:160
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double getB() const
Definition: vpPlane.h:104
std::vector< std::vector< double > > getFeaturesForDisplay()
double getA() const
Definition: vpPlane.h:102
void addPolygon(const int &index)
unsigned int getHeight() const
Definition: vpImage.h:188
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:87
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:100
std::vector< vpMbtMeLine * > meline
The moving edge container.
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpLine.cpp:333
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setB(double b)
Definition: vpPlane.h:84
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
vpMatrix L
The interaction matrix.
std::vector< unsigned int > nbFeature
The number of moving edges.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
bool useScanLine
Use scanline rendering.
double getD() const
Definition: vpPlane.h:108
void computeFov(const unsigned int &w, const unsigned int &h)