Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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(...) {
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(...) {
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  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, const unsigned int thickness,
640  const 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 *line = meline[i];
694  if (line != NULL) {
695  for (std::list<vpMeSite>::const_iterator it = line->getMeList().begin(); it != line->getMeList().end(); ++it) {
696  vpMeSite p_me = *it;
697 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
698  std::vector<double> params = {0, //ME
699  p_me.get_ifloat(),
700  p_me.get_jfloat(),
701  static_cast<double>(p_me.getState())};
702 #else
703  std::vector<double> params;
704  params.push_back(0); // ME
705  params.push_back(p_me.get_ifloat());
706  params.push_back(p_me.get_jfloat());
707  params.push_back(static_cast<double>(p_me.getState()));
708 #endif
709  features.push_back(params);
710  }
711  }
712  }
713 
714  return features;
715 }
716 
728 std::vector<std::vector<double> > vpMbtDistanceLine::getModelForDisplay(unsigned int width, unsigned int height,
729  const vpHomogeneousMatrix &cMo,
730  const vpCameraParameters &camera,
731  const bool displayFullModel)
732 {
733  std::vector<std::vector<double> > models;
734 
735  if ((isvisible && isTrackedLine) || displayFullModel) {
736  p1->changeFrame(cMo);
737  p2->changeFrame(cMo);
738 
739  vpImagePoint ip1, ip2;
740  vpCameraParameters c = camera;
741  if (poly.getClipping() > 3) // Contains at least one FOV constraint
742  c.computeFov(width, height);
743 
744  poly.computePolygonClipped(c);
745 
746  if (poly.polyClipped.size() == 2 &&
747  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
748  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
749  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
750  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
751  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
752  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
753 
754  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
755  if (useScanLine && !displayFullModel) {
756  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
757  } else {
758  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
759  }
760 
761  for (unsigned int i = 0; i < linesLst.size(); i++) {
762  linesLst[i].first.project();
763  linesLst[i].second.project();
764 
765  vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
766  vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
767 
768 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
769  std::vector<double> params = {0, //0 for line parameters
770  ip1.get_i(),
771  ip1.get_j(),
772  ip2.get_i(),
773  ip2.get_j()};
774 #else
775  std::vector<double> params;
776  params.push_back(0); //0 for line parameters
777  params.push_back(ip1.get_i());
778  params.push_back(ip1.get_j());
779  params.push_back(ip2.get_i());
780  params.push_back(ip2.get_j());
781 #endif
782  models.push_back(params);
783  }
784  }
785  }
786 
787  return models;
788 }
789 
794 {
795  if (isvisible) {
796  L.resize(nbFeatureTotal, 6);
798  } else {
799  for (size_t i = 0; i < meline.size(); i++) {
800  nbFeature[i] = 0;
801  // To be consistent with nbFeature[i] = 0
802  std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
803  me_site_list.clear();
804  }
805  nbFeatureTotal = 0;
806  }
807 }
808 
814 {
815  if (isvisible) {
816  try {
817  // feature projection
818  line->changeFrame(cMo);
819  line->projection();
820 
821  vpFeatureBuilder::create(featureline, *line);
822 
823  double rho = featureline.getRho();
824  double theta = featureline.getTheta();
825 
826  double co = cos(theta);
827  double si = sin(theta);
828 
829  double mx = 1.0 / cam.get_px();
830  double my = 1.0 / cam.get_py();
831  double xc = cam.get_u0();
832  double yc = cam.get_v0();
833 
834  double alpha_;
835  vpMatrix H = featureline.interaction();
836 
837  double x, y;
838  unsigned int j = 0;
839 
840  for (size_t i = 0; i < meline.size(); i++) {
841  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
842  it != meline[i]->getMeList().end(); ++it) {
843  x = (double)it->j;
844  y = (double)it->i;
845 
846  x = (x - xc) * mx;
847  y = (y - yc) * my;
848 
849  alpha_ = x * si - y * co;
850 
851  double *Lrho = H[0];
852  double *Ltheta = H[1];
853  // Calculate interaction matrix for a distance
854  for (unsigned int k = 0; k < 6; k++) {
855  L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
856  }
857  error[j] = rho - (x * co + y * si);
858  j++;
859  }
860  }
861  } catch (...) {
862  // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
863  // Set the corresponding interaction matrix part to zero
864  unsigned int j = 0;
865  for (size_t i = 0; i < meline.size(); i++) {
866  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
867  it != meline[i]->getMeList().end(); ++it) {
868  for (unsigned int k = 0; k < 6; k++) {
869  L[j][k] = 0.0;
870  }
871 
872  error[j] = 0.0;
873  j++;
874  }
875  }
876  }
877  }
878 }
879 
888 bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
889 {
890  if (threshold > I.getWidth() || threshold > I.getHeight()) {
891  return true;
892  }
893  if (isvisible) {
894 
895  for (size_t i = 0; i < meline.size(); i++) {
896  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
897  ++it) {
898  int i_ = it->i;
899  int j_ = it->j;
900 
901  if (i_ < 0 || j_ < 0) { // out of image.
902  return true;
903  }
904 
905  if (((unsigned int)i_ > (I.getHeight() - threshold)) || (unsigned int)i_ < threshold ||
906  ((unsigned int)j_ > (I.getWidth() - threshold)) || (unsigned int)j_ < threshold) {
907  return true;
908  }
909  }
910  }
911  }
912  return false;
913 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
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.
double get_jfloat() const
Definition: vpMeSite.h:166
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:237
bool Reinit
Indicates if the line has to be reinitialized.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:351
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
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)
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
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
vpMeSiteState getState() const
Definition: vpMeSite.h:189
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:130
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
double get_ifloat() const
Definition: vpMeSite.h:159
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
double getB() const
Definition: vpPlane.h:104
std::vector< std::vector< double > > getFeaturesForDisplay()
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:179
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< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
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:310
void computeFov(const unsigned int &w, const unsigned int &h)