Visual Servoing Platform  version 3.6.1 under development (2024-04-22)
vpMbtDistanceLine.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Make the complete tracking of an object by using its CAD model
33  *
34  * Authors:
35  * Romain Tallonneau
36  *
37 *****************************************************************************/
38 #include <visp3/core/vpConfig.h>
39 
45 #include <stdlib.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPlane.h>
48 #include <visp3/mbt/vpMbtDistanceLine.h>
49 #include <visp3/visual_features/vpFeatureBuilder.h>
50 
51 void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
52 void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);
53 
58  : name(), index(0), cam(), me(nullptr), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
59  poly(), useScanLine(false), meline(), line(nullptr), p1(nullptr), p2(nullptr), L(), error(), nbFeature(), nbFeatureTotal(0),
60  Reinit(false), hiddenface(nullptr), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
61 { }
62 
67 {
68  if (line != nullptr)
69  delete line;
70 
71  for (unsigned int i = 0; i < meline.size(); i++)
72  if (meline[i] != nullptr)
73  delete meline[i];
74 
75  meline.clear();
76 }
77 
84 void vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
85 {
86  line->project(cMo);
87  p1->project(cMo);
88  p2->project(cMo);
89 }
90 
100 void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
101 {
102  vpColVector a(3);
103  vpColVector b(3);
104 
105  // Calculate vector corresponding to PQ
106  a[0] = P.get_oX() - Q.get_oX();
107  a[1] = P.get_oY() - Q.get_oY();
108  a[2] = P.get_oZ() - Q.get_oZ();
109 
110  // Calculate vector corresponding to PR
111  b[0] = P.get_oX() - R.get_oX();
112  b[1] = P.get_oY() - R.get_oY();
113  b[2] = P.get_oZ() - R.get_oZ();
114 
115  // Calculate normal vector to plane PQ x PR
117 
118  // Equation of the plane is given by:
119  double A = n[0];
120  double B = n[1];
121  double C = n[2];
122  double D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
123 
124  double norm = sqrt(A * A + B * B + C * C);
125  plane.setA(A / norm);
126  plane.setB(B / norm);
127  plane.setC(C / norm);
128  plane.setD(D / norm);
129 }
130 
144 void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
145 {
146  vpPlane plane1;
147  vpPlane plane2;
148  buildPlane(P1, P2, P3, plane1);
149  buildPlane(P1, P2, P4, plane2);
150 
151  L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), plane2.getA(), plane2.getB(),
152  plane2.getC(), plane2.getD());
153 }
154 
164 {
165  if (line == nullptr) {
166  line = new vpLine;
167  }
168 
169  poly.setNbPoint(2);
170  poly.addPoint(0, _p1);
171  poly.addPoint(1, _p2);
172 
173  p1 = &poly.p[0];
174  p2 = &poly.p[1];
175 
176  vpColVector V1(3);
177  vpColVector V2(3);
178 
179  V1[0] = p1->get_oX();
180  V1[1] = p1->get_oY();
181  V1[2] = p1->get_oZ();
182  V2[0] = p2->get_oX();
183  V2[1] = p2->get_oY();
184  V2[2] = p2->get_oZ();
185 
186  // if((V1-V2).sumSquare()!=0)
187  if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
188  vpColVector V3(3);
189  V3[0] = double(rand_gen.next() % 1000) / 100;
190  V3[1] = double(rand_gen.next() % 1000) / 100;
191  V3[2] = double(rand_gen.next() % 1000) / 100;
192 
193  vpColVector v_tmp1, v_tmp2;
194  v_tmp1 = V2 - V1;
195  v_tmp2 = V3 - V1;
196  vpColVector V4 = vpColVector::cross(v_tmp1, v_tmp2);
197 
198  vpPoint P3(V3[0], V3[1], V3[2]);
199  vpPoint P4(V4[0], V4[1], V4[2]);
200  buildLine(*p1, *p2, P3, P4, *line);
201  }
202  else {
203  vpPoint P3(V1[0], V1[1], V1[2]);
204  vpPoint P4(V2[0], V2[1], V2[2]);
205  buildLine(*p1, *p2, P3, P4, *line);
206  }
207 }
208 
214 void vpMbtDistanceLine::addPolygon(const int &idx)
215 {
216  Lindex_polygon.push_back(idx);
217  Lindex_polygon_tracked.push_back(true);
218 }
219 
227 void vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
228 {
229  unsigned int ind = 0;
230  for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
231  if ((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) {
232  Lindex_polygon_tracked[ind] = track;
233  }
234  ind++;
235  }
236 
237  isTrackedLine = false;
238  for (unsigned int i = 0; i < Lindex_polygon_tracked.size(); i++)
239  if (Lindex_polygon_tracked[i]) {
240  isTrackedLine = true;
241  break;
242  }
243 
244  if (!isTrackedLine) {
245  isTrackedLineWithVisibility = false;
246  return;
247  }
248 
249  updateTracked();
250 }
251 
257 {
258  if (!isTrackedLine) {
259  isTrackedLineWithVisibility = false;
260  return;
261  }
262 
263  unsigned int ind = 0;
264  isTrackedLineWithVisibility = false;
265  for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
266  if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) {
267  isTrackedLineWithVisibility = true;
268  break;
269  }
270  ind++;
271  }
272 }
273 
280 {
281  me = _me;
282 
283  for (unsigned int i = 0; i < meline.size(); i++)
284  if (meline[i] != nullptr) {
285  // nbFeature[i] = 0;
286  meline[i]->reset();
287  meline[i]->setMe(me);
288  }
289 
290  // nbFeatureTotal = 0;
291 }
292 
305  const vpImage<bool> *mask)
306 {
307  for (unsigned int i = 0; i < meline.size(); i++) {
308  if (meline[i] != nullptr)
309  delete meline[i];
310  }
311 
312  meline.clear();
313  nbFeature.clear();
314  nbFeatureTotal = 0;
315 
316  if (isvisible) {
317  p1->changeFrame(cMo);
318  p2->changeFrame(cMo);
319 
320  if (poly.getClipping() > 3) // Contains at least one FOV constraint
321  cam.computeFov(I.getWidth(), I.getHeight());
322 
323  poly.computePolygonClipped(cam);
324 
325  if (poly.polyClipped.size() == 2) { // Les points sont visibles.
326 
327  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
328 
329  if (useScanLine) {
330  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
331  }
332  else {
333  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
334  }
335 
336  if (linesLst.size() == 0) {
337  return false;
338  }
339 
340  line->changeFrame(cMo);
341  try {
342  line->projection();
343  }
344  catch (...) {
345  isvisible = false;
346  return false;
347  }
348  double rho, theta;
349  // rho theta uv
351 
352  while (theta > M_PI) {
353  theta -= M_PI;
354  }
355  while (theta < -M_PI) {
356  theta += M_PI;
357  }
358 
359  if (theta < -M_PI / 2.0)
360  theta = -theta - 3 * M_PI / 2.0;
361  else
362  theta = M_PI / 2.0 - theta;
363 
364  for (unsigned int i = 0; i < linesLst.size(); i++) {
365  vpImagePoint ip1, ip2;
366 
367  linesLst[i].first.project();
368  linesLst[i].second.project();
369 
370  vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
371  vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
372 
373  vpMbtMeLine *melinePt = new vpMbtMeLine;
374  melinePt->setMask(*mask);
375  melinePt->setMe(me);
376 
377  melinePt->setInitRange(0);
378 
379  int marge = /*10*/ 5; // ou 5 normalement
380  if (ip1.get_j() < ip2.get_j()) {
381  melinePt->jmin = (int)ip1.get_j() - marge;
382  melinePt->jmax = (int)ip2.get_j() + marge;
383  }
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  }
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  }
403  catch (...) {
404  delete melinePt;
405  isvisible = false;
406  return false;
407  }
408  }
409  }
410  else {
411  isvisible = false;
412  }
413  }
414 
415  return true;
416 }
417 
424 {
425  if (isvisible) {
426  try {
427  nbFeature.clear();
428  nbFeatureTotal = 0;
429  for (size_t i = 0; i < meline.size(); i++) {
430  meline[i]->track(I);
431  nbFeature.push_back((unsigned int)meline[i]->getMeList().size());
432  nbFeatureTotal += (unsigned int)meline[i]->getMeList().size();
433  }
434  }
435  catch (...) {
436  for (size_t i = 0; i < meline.size(); i++) {
437  if (meline[i] != nullptr)
438  delete meline[i];
439  }
440 
441  nbFeature.clear();
442  meline.clear();
443  nbFeatureTotal = 0;
444  Reinit = true;
445  isvisible = false;
446  }
447  }
448 }
449 
457 {
458  if (isvisible) {
459  p1->changeFrame(cMo);
460  p2->changeFrame(cMo);
461 
462  if (poly.getClipping() > 3) // Contains at least one FOV constraint
463  cam.computeFov(I.getWidth(), I.getHeight());
464 
465  poly.computePolygonClipped(cam);
466 
467  if (poly.polyClipped.size() == 2) { // Les points sont visibles.
468 
469  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
470 
471  if (useScanLine) {
472  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
473  }
474  else {
475  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
476  }
477 
478  if (linesLst.size() != meline.size() || linesLst.size() == 0) {
479  for (size_t i = 0; i < meline.size(); i++) {
480  if (meline[i] != nullptr)
481  delete meline[i];
482  }
483 
484  meline.clear();
485  nbFeature.clear();
486  nbFeatureTotal = 0;
487  isvisible = false;
488  Reinit = true;
489  }
490  else {
491  line->changeFrame(cMo);
492  try {
493  line->projection();
494  }
495  catch (...) {
496  for (size_t j = 0; j < meline.size(); j++) {
497  if (meline[j] != nullptr)
498  delete meline[j];
499  }
500 
501  meline.clear();
502  nbFeature.clear();
503  nbFeatureTotal = 0;
504  isvisible = false;
505  Reinit = true;
506  return;
507  }
508  double rho, theta;
509  // rho theta uv
511 
512  while (theta > M_PI) {
513  theta -= M_PI;
514  }
515  while (theta < -M_PI) {
516  theta += M_PI;
517  }
518 
519  if (theta < -M_PI / 2.0)
520  theta = -theta - 3 * M_PI / 2.0;
521  else
522  theta = M_PI / 2.0 - theta;
523 
524  try {
525  for (unsigned int i = 0; i < linesLst.size(); i++) {
526  vpImagePoint ip1, ip2;
527 
528  linesLst[i].first.project();
529  linesLst[i].second.project();
530 
531  vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
532  vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
533 
534  int marge = /*10*/ 5; // ou 5 normalement
535  if (ip1.get_j() < ip2.get_j()) {
536  meline[i]->jmin = (int)ip1.get_j() - marge;
537  meline[i]->jmax = (int)ip2.get_j() + marge;
538  }
539  else {
540  meline[i]->jmin = (int)ip2.get_j() - marge;
541  meline[i]->jmax = (int)ip1.get_j() + marge;
542  }
543  if (ip1.get_i() < ip2.get_i()) {
544  meline[i]->imin = (int)ip1.get_i() - marge;
545  meline[i]->imax = (int)ip2.get_i() + marge;
546  }
547  else {
548  meline[i]->imin = (int)ip2.get_i() - marge;
549  meline[i]->imax = (int)ip1.get_i() + marge;
550  }
551 
552  meline[i]->updateParameters(I, ip1, ip2, rho, theta);
553  nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
555  }
556  }
557  catch (...) {
558  for (size_t j = 0; j < meline.size(); j++) {
559  if (meline[j] != nullptr)
560  delete meline[j];
561  }
562 
563  meline.clear();
564  nbFeature.clear();
565  nbFeatureTotal = 0;
566  isvisible = false;
567  Reinit = true;
568  }
569  }
570  }
571  else {
572  for (size_t i = 0; i < meline.size(); i++) {
573  if (meline[i] != nullptr)
574  delete meline[i];
575  }
576  nbFeature.clear();
577  meline.clear();
578  nbFeatureTotal = 0;
579  isvisible = false;
580  }
581  }
582 }
583 
596  const vpImage<bool> *mask)
597 {
598  for (size_t i = 0; i < meline.size(); i++) {
599  if (meline[i] != nullptr)
600  delete meline[i];
601  }
602 
603  nbFeature.clear();
604  meline.clear();
605  nbFeatureTotal = 0;
606 
607  if (!initMovingEdge(I, cMo, false, mask))
608  Reinit = true;
609 
610  Reinit = false;
611 }
612 
625  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
626  bool displayFullModel)
627 {
628  std::vector<std::vector<double> > models =
629  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
630 
631  for (size_t i = 0; i < models.size(); i++) {
632  vpImagePoint ip1(models[i][1], models[i][2]);
633  vpImagePoint ip2(models[i][3], models[i][4]);
634  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
635  }
636 }
637 
650  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
651  bool displayFullModel)
652 {
653  std::vector<std::vector<double> > models =
654  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
655 
656  for (size_t i = 0; i < models.size(); i++) {
657  vpImagePoint ip1(models[i][1], models[i][2]);
658  vpImagePoint ip2(models[i][3], models[i][4]);
659  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
660  }
661 }
662 
678 {
679  for (size_t i = 0; i < meline.size(); i++) {
680  if (meline[i] != nullptr) {
681  meline[i]->display(I);
682  }
683  }
684 }
685 
687 {
688  for (size_t i = 0; i < meline.size(); i++) {
689  if (meline[i] != nullptr) {
690  meline[i]->display(I);
691  }
692  }
693 }
694 
699 std::vector<std::vector<double> > vpMbtDistanceLine::getFeaturesForDisplay()
700 {
701  std::vector<std::vector<double> > features;
702 
703  for (size_t i = 0; i < meline.size(); i++) {
704  vpMbtMeLine *me_l = meline[i];
705  if (me_l != nullptr) {
706  for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
707  vpMeSite p_me_l = *it;
708 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
709  std::vector<double> params = { 0, //ME
710  p_me_l.get_ifloat(),
711  p_me_l.get_jfloat(),
712  static_cast<double>(p_me_l.getState()) };
713 #else
714  std::vector<double> params;
715  params.push_back(0); // ME
716  params.push_back(p_me_l.get_ifloat());
717  params.push_back(p_me_l.get_jfloat());
718  params.push_back(static_cast<double>(p_me_l.getState()));
719 #endif
720 
721  features.push_back(params);
722  }
723  }
724  }
725 
726  return features;
727 }
728 
740 std::vector<std::vector<double> > vpMbtDistanceLine::getModelForDisplay(unsigned int width, unsigned int height,
741  const vpHomogeneousMatrix &cMo,
742  const vpCameraParameters &camera,
743  bool displayFullModel)
744 {
745  std::vector<std::vector<double> > models;
746 
747  if ((isvisible && isTrackedLine) || displayFullModel) {
748  p1->changeFrame(cMo);
749  p2->changeFrame(cMo);
750 
751  vpImagePoint ip1, ip2;
752  vpCameraParameters c = camera;
753  if (poly.getClipping() > 3) // Contains at least one FOV constraint
754  c.computeFov(width, height);
755 
756  poly.computePolygonClipped(c);
757 
758  if (poly.polyClipped.size() == 2 &&
759  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
760  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
761  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
762  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
763  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
764  ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
765 
766  std::vector<std::pair<vpPoint, vpPoint> > linesLst;
767  if (useScanLine && !displayFullModel) {
768  hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
769  }
770  else {
771  linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
772  }
773 
774  for (unsigned int i = 0; i < linesLst.size(); i++) {
775  linesLst[i].first.project();
776  linesLst[i].second.project();
777 
778  vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
779  vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
780 
781 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
782  std::vector<double> params = { 0, //0 for line parameters
783  ip1.get_i(),
784  ip1.get_j(),
785  ip2.get_i(),
786  ip2.get_j() };
787 #else
788  std::vector<double> params;
789  params.push_back(0); //0 for line parameters
790  params.push_back(ip1.get_i());
791  params.push_back(ip1.get_j());
792  params.push_back(ip2.get_i());
793  params.push_back(ip2.get_j());
794 #endif
795 
796  models.push_back(params);
797  }
798  }
799  }
800 
801  return models;
802 }
803 
808 {
809  if (isvisible) {
810  L.resize(nbFeatureTotal, 6);
812  }
813  else {
814  for (size_t i = 0; i < meline.size(); i++) {
815  nbFeature[i] = 0;
816  // To be consistent with nbFeature[i] = 0
817  std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
818  me_site_list.clear();
819  }
820  nbFeatureTotal = 0;
821  }
822 }
823 
829 {
830  if (isvisible) {
831  try {
832  // feature projection
833  line->changeFrame(cMo);
834  line->projection();
835 
836  vpFeatureBuilder::create(featureline, *line);
837 
838  double rho = featureline.getRho();
839  double theta = featureline.getTheta();
840 
841  double co = cos(theta);
842  double si = sin(theta);
843 
844  double mx = 1.0 / cam.get_px();
845  double my = 1.0 / cam.get_py();
846  double xc = cam.get_u0();
847  double yc = cam.get_v0();
848 
849  double alpha_;
850  vpMatrix H = featureline.interaction();
851 
852  double x, y;
853  unsigned int j = 0;
854 
855  for (size_t i = 0; i < meline.size(); i++) {
856  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
857  it != meline[i]->getMeList().end(); ++it) {
858  x = (double)it->m_j;
859  y = (double)it->m_i;
860 
861  x = (x - xc) * mx;
862  y = (y - yc) * my;
863 
864  alpha_ = x * si - y * co;
865 
866  double *Lrho = H[0];
867  double *Ltheta = H[1];
868  // Calculate interaction matrix for a distance
869  for (unsigned int k = 0; k < 6; k++) {
870  L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
871  }
872  error[j] = rho - (x * co + y * si);
873  j++;
874  }
875  }
876  }
877  catch (...) {
878  // Handle potential exception: due to a degenerate case: the image of the straight line is a point!
879  // Set the corresponding interaction matrix part to zero
880  unsigned int j = 0;
881  for (size_t i = 0; i < meline.size(); i++) {
882  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
883  it != meline[i]->getMeList().end(); ++it) {
884  for (unsigned int k = 0; k < 6; k++) {
885  L[j][k] = 0.0;
886  }
887 
888  error[j] = 0.0;
889  j++;
890  }
891  }
892  }
893  }
894 }
895 
904 bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
905 {
906  if (threshold > I.getWidth() || threshold > I.getHeight()) {
907  return true;
908  }
909  if (isvisible) {
910 
911  for (size_t i = 0; i < meline.size(); i++) {
912  for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
913  ++it) {
914  int i_ = it->m_i;
915  int j_ = it->m_j;
916 
917  if (i_ < 0 || j_ < 0) { // out of image.
918  return true;
919  }
920 
921  if (((unsigned int)i_ >(I.getHeight() - threshold)) || (unsigned int)i_ < threshold ||
922  ((unsigned int)j_ >(I.getWidth() - threshold)) || (unsigned int)j_ < threshold) {
923  return true;
924  }
925  }
926  }
927  }
928  return false;
929 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:352
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:163
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:1172
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
double getTheta() const
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
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:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:101
double getRho() const
Definition: vpLine.h:130
double getTheta() const
Definition: vpLine.h:141
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpLine.cpp:327
void projection() vp_override
Definition: vpLine.cpp:190
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
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 reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
void setTracked(const std::string &name, const bool &track)
void addPolygon(const int &index)
void trackMovingEdge(const vpImage< unsigned char > &I)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
vpMeSiteState getState() const
Definition: vpMeSite.h:266
double get_ifloat() const
Definition: vpMeSite.h:192
double get_jfloat() const
Definition: vpMeSite.h:198
Definition: vpMe.h:124
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
void setA(double a)
Definition: vpPlane.h:80
void setD(double d)
Definition: vpPlane.h:86
double getD() const
Definition: vpPlane.h:106
void setC(double c)
Definition: vpPlane.h:84
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
void setB(double b)
Definition: vpPlane.h:82
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:454
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:458
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:456
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpPoint.cpp:242
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:76
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
virtual void setNbPoint(unsigned int nb)
unsigned int getClipping() const
Definition: vpPolygon3D.h:113
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
Definition: vpPolygon3D.h:78
void addPoint(unsigned int n, const vpPoint &P)
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:123
uint32_t next()
Definition: vpUniRand.cpp:145