Visual Servoing Platform  version 3.6.0 under development (2023-09-25)
vpMbtDistanceCylinder.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. Cylinder
33  * tracking.
34  *
35  * Authors:
36  * Romain Tallonneau
37  * Bertrand Delabarre
38  *
39 *****************************************************************************/
40 
41 #include <visp3/core/vpConfig.h>
42 
48 #include <algorithm>
49 #include <stdlib.h>
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPlane.h>
53 #include <visp3/mbt/vpMbtDistanceCylinder.h>
54 #include <visp3/visual_features/vpFeatureBuilder.h>
55 #include <visp3/visual_features/vpFeatureEllipse.h>
56 
57 #include <visp3/vision/vpPose.h>
58 
63  : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1), featureline1(), featureline2(), isTrackedCylinder(true),
64  meline1(NULL), meline2(NULL), cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(), error(),
65  nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(NULL), hiddenface(NULL), index_polygon(-1),
66  isvisible(false)
67 {
68 }
69 
74 {
75  if (p1 != NULL)
76  delete p1;
77  if (p2 != NULL)
78  delete p2;
79  if (c != NULL)
80  delete c;
81  if (meline1 != NULL)
82  delete meline1;
83  if (meline2 != NULL)
84  delete meline2;
85  if (cercle1 != NULL)
86  delete cercle1;
87  if (cercle2 != NULL)
88  delete cercle2;
89 }
90 
98 void vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
99 {
100  c->project(cMo);
101  p1->project(cMo);
102  p2->project(cMo);
103  cercle1->project(cMo);
104  cercle2->project(cMo);
105 }
106 
115 void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
116 {
117  c = new vpCylinder;
118  p1 = new vpPoint;
119  p2 = new vpPoint;
120  cercle1 = new vpCircle;
121  cercle2 = new vpCircle;
122 
123  // Get the points
124  *p1 = _p1;
125  *p2 = _p2;
126 
127  // Get the radius
128  radius = r;
129 
130  vpColVector ABC(3);
131  vpColVector V1(3);
132  vpColVector V2(3);
133 
134  V1[0] = _p1.get_oX();
135  V1[1] = _p1.get_oY();
136  V1[2] = _p1.get_oZ();
137  V2[0] = _p2.get_oX();
138  V2[1] = _p2.get_oY();
139  V2[2] = _p2.get_oZ();
140 
141  // Get the axis of the cylinder
142  ABC = V1 - V2;
143 
144  // Build our extremity circles
145  cercle1->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
146  cercle2->setWorldCoordinates(ABC[0], ABC[1], ABC[2], _p2.get_oX(), _p2.get_oY(), _p2.get_oZ(), r);
147 
148  // Build our cylinder
149  c->setWorldCoordinates(ABC[0], ABC[1], ABC[2], (_p1.get_oX() + _p2.get_oX()) / 2.0,
150  (_p1.get_oY() + _p2.get_oY()) / 2.0, (_p1.get_oZ() + _p2.get_oZ()) / 2.0, r);
151 }
152 
159 {
160  me = _me;
161  if (meline1 != NULL) {
162  meline1->setMe(me);
163  }
164  if (meline2 != NULL) {
165  meline2->setMe(me);
166  }
167 }
168 
181  bool doNotTrack, const vpImage<bool> *mask)
182 {
183  if (isvisible) {
184  // Perspective projection
185  p1->changeFrame(cMo);
186  p2->changeFrame(cMo);
187  cercle1->changeFrame(cMo);
188  cercle2->changeFrame(cMo);
189  c->changeFrame(cMo);
190 
191  p1->projection();
192  p2->projection();
193  try {
194  cercle1->projection();
195  } catch (...) {
196  // std::cout<<"Problem when projecting circle 1\n";
197  return false;
198  }
199  try {
200  cercle2->projection();
201  } catch (...) {
202  // std::cout<<"Problem when projecting circle 2\n";
203  return false;
204  }
205  c->projection();
206 
207  double rho1, theta1;
208  double rho2, theta2;
209 
210  // Create the moving edges containers
211  meline1 = new vpMbtMeLine;
212  meline1->setMask(*mask);
213  meline1->setMe(me);
214  meline2 = new vpMbtMeLine;
215  meline1->setMask(*mask);
216  meline2->setMe(me);
217 
218  // meline->setDisplay(vpMeSite::RANGE_RESULT);
219  meline1->setInitRange(0);
220  meline2->setInitRange(0);
221 
222  // Conversion meter to pixels
223  vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
224  vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
225 
226  // Determine intersections between circles and limbos
227  double i11, i12, i21, i22, j11, j12, j21, j22;
228  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
229  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
230  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
231  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
232 
233  // Create the image points
234  vpImagePoint ip11, ip12, ip21, ip22;
235  ip11.set_ij(i11, j11);
236  ip12.set_ij(i12, j12);
237  ip21.set_ij(i21, j21);
238  ip22.set_ij(i22, j22);
239 
240  // update limits of the melines.
241  int marge = /*10*/ 5; // ou 5 normalement
242  if (ip11.get_j() < ip12.get_j()) {
243  meline1->jmin = (int)ip11.get_j() - marge;
244  meline1->jmax = (int)ip12.get_j() + marge;
245  } else {
246  meline1->jmin = (int)ip12.get_j() - marge;
247  meline1->jmax = (int)ip11.get_j() + marge;
248  }
249  if (ip11.get_i() < ip12.get_i()) {
250  meline1->imin = (int)ip11.get_i() - marge;
251  meline1->imax = (int)ip12.get_i() + marge;
252  } else {
253  meline1->imin = (int)ip12.get_i() - marge;
254  meline1->imax = (int)ip11.get_i() + marge;
255  }
256 
257  if (ip21.get_j() < ip22.get_j()) {
258  meline2->jmin = (int)ip21.get_j() - marge;
259  meline2->jmax = (int)ip22.get_j() + marge;
260  } else {
261  meline2->jmin = (int)ip22.get_j() - marge;
262  meline2->jmax = (int)ip21.get_j() + marge;
263  }
264  if (ip21.get_i() < ip22.get_i()) {
265  meline2->imin = (int)ip21.get_i() - marge;
266  meline2->imax = (int)ip22.get_i() + marge;
267  } else {
268  meline2->imin = (int)ip22.get_i() - marge;
269  meline2->imax = (int)ip21.get_i() + marge;
270  }
271 
272  // Initialize the tracking
273  while (theta1 > M_PI) {
274  theta1 -= M_PI;
275  }
276  while (theta1 < -M_PI) {
277  theta1 += M_PI;
278  }
279 
280  if (theta1 < -M_PI / 2.0)
281  theta1 = -theta1 - 3 * M_PI / 2.0;
282  else
283  theta1 = M_PI / 2.0 - theta1;
284 
285  while (theta2 > M_PI) {
286  theta2 -= M_PI;
287  }
288  while (theta2 < -M_PI) {
289  theta2 += M_PI;
290  }
291 
292  if (theta2 < -M_PI / 2.0)
293  theta2 = -theta2 - 3 * M_PI / 2.0;
294  else
295  theta2 = M_PI / 2.0 - theta2;
296 
297  try {
298  meline1->initTracking(I, ip11, ip12, rho1, theta1, doNotTrack);
299  } catch (...) {
300  // vpTRACE("the line can't be initialized");
301  return false;
302  }
303  try {
304  meline2->initTracking(I, ip21, ip22, rho2, theta2, doNotTrack);
305  } catch (...) {
306  // vpTRACE("the line can't be initialized");
307  return false;
308  }
309  }
310  return true;
311 }
312 
320 {
321  if (isvisible) {
322  try {
323  meline1->track(I);
324  } catch (...) {
325  // std::cout << "Track meline1 failed" << std::endl;
326  meline1->reset();
327  Reinit = true;
328  }
329  try {
330  meline2->track(I);
331  } catch (...) {
332  // std::cout << "Track meline2 failed" << std::endl;
333  meline2->reset();
334  Reinit = true;
335  }
336 
337  // Update the number of features
338  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
339  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
341  }
342 }
343 
351 {
352  if (isvisible) {
353  // Perspective projection
354  p1->changeFrame(cMo);
355  p2->changeFrame(cMo);
356  cercle1->changeFrame(cMo);
357  cercle2->changeFrame(cMo);
358  c->changeFrame(cMo);
359 
360  p1->projection();
361  p2->projection();
362  try {
363  cercle1->projection();
364  } catch (...) {
365  std::cout << "Probleme projection cercle 1\n";
366  }
367  try {
368  cercle2->projection();
369  } catch (...) {
370  std::cout << "Probleme projection cercle 2\n";
371  }
372  c->projection();
373 
374  // Get the limbos
375  double rho1, theta1;
376  double rho2, theta2;
377 
378  // Conversion meter to pixels
379  vpMeterPixelConversion::convertLine(cam, c->getRho1(), c->getTheta1(), rho1, theta1);
380  vpMeterPixelConversion::convertLine(cam, c->getRho2(), c->getTheta2(), rho2, theta2);
381 
382  // Determine intersections between circles and limbos
383  double i11, i12, i21, i22, j11, j12, j21, j22;
384 
385  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
386  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
387 
388  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
389  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
390 
391  // Create the image points
392  vpImagePoint ip11, ip12, ip21, ip22;
393  ip11.set_ij(i11, j11);
394  ip12.set_ij(i12, j12);
395  ip21.set_ij(i21, j21);
396  ip22.set_ij(i22, j22);
397 
398  // update limits of the meline.
399  int marge = /*10*/ 5; // ou 5 normalement
400  if (ip11.get_j() < ip12.get_j()) {
401  meline1->jmin = (int)ip11.get_j() - marge;
402  meline1->jmax = (int)ip12.get_j() + marge;
403  } else {
404  meline1->jmin = (int)ip12.get_j() - marge;
405  meline1->jmax = (int)ip11.get_j() + marge;
406  }
407  if (ip11.get_i() < ip12.get_i()) {
408  meline1->imin = (int)ip11.get_i() - marge;
409  meline1->imax = (int)ip12.get_i() + marge;
410  } else {
411  meline1->imin = (int)ip12.get_i() - marge;
412  meline1->imax = (int)ip11.get_i() + marge;
413  }
414 
415  if (ip21.get_j() < ip22.get_j()) {
416  meline2->jmin = (int)ip21.get_j() - marge;
417  meline2->jmax = (int)ip22.get_j() + marge;
418  } else {
419  meline2->jmin = (int)ip22.get_j() - marge;
420  meline2->jmax = (int)ip21.get_j() + marge;
421  }
422  if (ip21.get_i() < ip22.get_i()) {
423  meline2->imin = (int)ip21.get_i() - marge;
424  meline2->imax = (int)ip22.get_i() + marge;
425  } else {
426  meline2->imin = (int)ip22.get_i() - marge;
427  meline2->imax = (int)ip21.get_i() + marge;
428  }
429 
430  // Initialize the tracking
431  while (theta1 > M_PI) {
432  theta1 -= M_PI;
433  }
434  while (theta1 < -M_PI) {
435  theta1 += M_PI;
436  }
437 
438  if (theta1 < -M_PI / 2.0)
439  theta1 = -theta1 - 3 * M_PI / 2.0;
440  else
441  theta1 = M_PI / 2.0 - theta1;
442 
443  while (theta2 > M_PI) {
444  theta2 -= M_PI;
445  }
446  while (theta2 < -M_PI) {
447  theta2 += M_PI;
448  }
449 
450  if (theta2 < -M_PI / 2.0)
451  theta2 = -theta2 - 3 * M_PI / 2.0;
452  else
453  theta2 = M_PI / 2.0 - theta2;
454 
455  try {
456  // meline1->updateParameters(I,rho1,theta1);
457  meline1->updateParameters(I, ip11, ip12, rho1, theta1);
458  } catch (...) {
459  Reinit = true;
460  }
461  try {
462  // meline2->updateParameters(I,rho2,theta2);
463  meline2->updateParameters(I, ip21, ip22, rho2, theta2);
464  } catch (...) {
465  Reinit = true;
466  }
467 
468  // Update the numbers of features
469  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
470  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
472  }
473 }
474 
487  const vpImage<bool> *mask)
488 {
489  if (meline1 != NULL)
490  delete meline1;
491  if (meline2 != NULL)
492  delete meline2;
493 
494  meline1 = NULL;
495  meline2 = NULL;
496 
497  if (!initMovingEdge(I, cMo, false, mask))
498  Reinit = true;
499 
500  Reinit = false;
501 }
502 
514  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
515  bool displayFullModel)
516 {
517  std::vector<std::vector<double> > models =
518  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
519 
520  for (size_t i = 0; i < models.size(); i++) {
521  vpImagePoint ip1(models[i][1], models[i][2]);
522  vpImagePoint ip2(models[i][3], models[i][4]);
523 
524  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
525  }
526 }
527 
539  const vpCameraParameters &camera, const vpColor &col, unsigned int thickness,
540  bool displayFullModel)
541 {
542  std::vector<std::vector<double> > models =
543  getModelForDisplay(I.getWidth(), I.getHeight(), cMo, camera, displayFullModel);
544 
545  for (size_t i = 0; i < models.size(); i++) {
546  vpImagePoint ip1(models[i][1], models[i][2]);
547  vpImagePoint ip2(models[i][3], models[i][4]);
548 
549  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
550  }
551 }
552 
557 std::vector<std::vector<double> > vpMbtDistanceCylinder::getFeaturesForDisplay()
558 {
559  std::vector<std::vector<double> > features;
560 
561  if (meline1 != NULL) {
562  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
563  ++it) {
564  vpMeSite p_me = *it;
565 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
566  std::vector<double> params = {0, // ME
567  p_me.get_ifloat(), p_me.get_jfloat(), static_cast<double>(p_me.getState())};
568 #else
569  std::vector<double> params;
570  params.push_back(0); // ME
571  params.push_back(p_me.get_ifloat());
572  params.push_back(p_me.get_jfloat());
573  params.push_back(static_cast<double>(p_me.getState()));
574 #endif
575  features.push_back(params);
576  }
577  }
578 
579  if (meline2 != NULL) {
580  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
581  ++it) {
582  vpMeSite p_me = *it;
583 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
584  std::vector<double> params = {0, // ME
585  p_me.get_ifloat(), p_me.get_jfloat(), static_cast<double>(p_me.getState())};
586 #else
587  std::vector<double> params;
588  params.push_back(0); // ME
589  params.push_back(p_me.get_ifloat());
590  params.push_back(p_me.get_jfloat());
591  params.push_back(static_cast<double>(p_me.getState()));
592 #endif
593  features.push_back(params);
594  }
595  }
596 
597  return features;
598 }
599 
610 std::vector<std::vector<double> > vpMbtDistanceCylinder::getModelForDisplay(unsigned int, unsigned int,
611  const vpHomogeneousMatrix &cMo,
612  const vpCameraParameters &camera,
613  bool displayFullModel)
614 {
615  std::vector<std::vector<double> > models;
616 
617  if ((isvisible && isTrackedCylinder) || displayFullModel) {
618  // Perspective projection
619  p1->changeFrame(cMo);
620  p2->changeFrame(cMo);
621  cercle1->changeFrame(cMo);
622  cercle2->changeFrame(cMo);
623  c->changeFrame(cMo);
624 
625  p1->projection();
626  p2->projection();
627  try {
628  cercle1->projection();
629  } catch (...) {
630  std::cout << "Problem projection circle 1";
631  }
632  try {
633  cercle2->projection();
634  } catch (...) {
635  std::cout << "Problem projection circle 2";
636  }
637  c->projection();
638 
639  double rho1, theta1;
640  double rho2, theta2;
641 
642  // Meters to pixels conversion
643  vpMeterPixelConversion::convertLine(camera, c->getRho1(), c->getTheta1(), rho1, theta1);
644  vpMeterPixelConversion::convertLine(camera, c->getRho2(), c->getTheta2(), rho2, theta2);
645 
646  // Determine intersections between circles and limbos
647  double i11, i12, i21, i22, j11, j12, j21, j22;
648 
649  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
650  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
651 
652  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
653  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
654 
655  // Create the image points
656  vpImagePoint ip11, ip12, ip21, ip22;
657  ip11.set_ij(i11, j11);
658  ip12.set_ij(i12, j12);
659  ip21.set_ij(i21, j21);
660  ip22.set_ij(i22, j22);
661 
662 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
663  std::vector<double> params1 = {0, ip11.get_i(), ip11.get_j(), ip12.get_i(), ip12.get_j()};
664 
665  std::vector<double> params2 = {0, ip21.get_i(), ip21.get_j(), ip22.get_i(), ip22.get_j()};
666 #else
667  std::vector<double> params1, params2;
668  params1.push_back(0);
669  params1.push_back(ip11.get_i());
670  params1.push_back(ip11.get_j());
671  params1.push_back(ip12.get_i());
672  params1.push_back(ip12.get_j());
673 
674  params2.push_back(0);
675  params2.push_back(ip11.get_i());
676  params2.push_back(ip11.get_j());
677  params2.push_back(ip12.get_i());
678  params2.push_back(ip12.get_j());
679 #endif
680 
681  models.push_back(params1);
682  models.push_back(params2);
683  }
684 
685  return models;
686 }
687 
703 {
704  if (meline1 != NULL) {
705  meline1->display(I);
706  }
707  if (meline2 != NULL) {
708  meline2->display(I);
709  }
710 }
711 
713 {
714  if (meline1 != NULL) {
715  meline1->display(I);
716  }
717  if (meline2 != NULL) {
718  meline2->display(I);
719  }
720 }
721 
726 {
727  if (isvisible) {
728  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
729  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
731  L.resize(nbFeature, 6);
733  } else {
734  nbFeature = 0;
735  nbFeaturel1 = 0;
736  nbFeaturel2 = 0;
737  }
738 }
739 
745  const vpImage<unsigned char> &I)
746 {
747  if (isvisible) {
748  // Perspective projection
749  c->changeFrame(cMo);
750  c->projection();
751  cercle1->changeFrame(cMo);
752  cercle1->changeFrame(cMo);
753  try {
754  cercle1->projection();
755  } catch (...) {
756  std::cout << "Problem projection circle 1\n";
757  }
758  try {
759  cercle2->projection();
760  } catch (...) {
761  std::cout << "Problem projection circle 2\n";
762  }
763 
764  bool disp = false;
765  bool disp2 = false;
766  if (disp || disp2)
767  vpDisplay::flush(I);
768 
769  // Build the lines
772 
773  double rho1 = featureline1.getRho();
774  double theta1 = featureline1.getTheta();
775  double rho2 = featureline2.getRho();
776  double theta2 = featureline2.getTheta();
777 
778  double co1 = cos(theta1);
779  double si1 = sin(theta1);
780  double co2 = cos(theta2);
781  double si2 = sin(theta2);
782 
783  double mx = 1.0 / cam.get_px();
784  double my = 1.0 / cam.get_py();
785  double xc = cam.get_u0();
786  double yc = cam.get_v0();
787 
788  vpMatrix H1;
789  H1 = featureline1.interaction();
790  vpMatrix H2;
791  H2 = featureline2.interaction();
792 
793  vpMeSite p;
794  unsigned int j = 0;
795  for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
796  ++it) {
797  double x = (double)it->j;
798  double y = (double)it->i;
799 
800  x = (x - xc) * mx;
801  y = (y - yc) * my;
802 
803  double alpha1 = x * si1 - y * co1;
804 
805  double *Lrho = H1[0];
806  double *Ltheta = H1[1];
807  // Calculate interaction matrix for a distance
808  for (unsigned int k = 0; k < 6; k++) {
809  L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
810  }
811  error[j] = rho1 - (x * co1 + y * si1);
812 
813  if (disp)
814  vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::orange, 1);
815 
816  j++;
817  }
818 
819  for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
820  ++it) {
821  double x = (double)it->j;
822  double y = (double)it->i;
823 
824  x = (x - xc) * mx;
825  y = (y - yc) * my;
826 
827  double alpha2 = x * si2 - y * co2;
828 
829  double *Lrho = H2[0];
830  double *Ltheta = H2[1];
831  // Calculate interaction matrix for a distance
832  for (unsigned int k = 0; k < 6; k++) {
833  L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
834  }
835  error[j] = rho2 - (x * co2 + y * si2);
836 
837  if (disp)
838  vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::red, 1);
839 
840  j++;
841  }
842  }
843 }
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:87
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
Definition: vpCircle.cpp:246
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:57
void projection()
Definition: vpCircle.cpp:137
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:396
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:351
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor orange
Definition: vpColor.h:221
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:98
double getRho1() const
Definition: vpCylinder.h:131
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCylinder.cpp:246
void projection()
Definition: vpCylinder.cpp:151
double getTheta1() const
Definition: vpCylinder.h:137
double getTheta2() const
Definition: vpCylinder.h:150
double getRho2() const
Definition: vpCylinder.h:144
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:62
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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
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:82
double get_j() const
Definition: vpImagePoint.h:125
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:313
double get_i() const
Definition: vpImagePoint.h:114
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:184
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
vpCylinder * c
The cylinder.
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
vpCircle * cercle1
The upper circle limiting the cylinder.
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void displayMovingEdges(const vpImage< unsigned char > &I)
std::vector< std::vector< double > > getFeaturesForDisplay()
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
unsigned int nbFeature
The number of moving edges.
vpCircle * cercle2
The lower circle limiting the cylinder.
bool isvisible
Indicates if the cylinder is visible or not.
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
int j
Coordinates along j of a site.
Definition: vpMeSite.h:98
vpMeSiteState getState() const
Definition: vpMeSite.h:261
double get_ifloat() const
Definition: vpMeSite.h:231
double get_jfloat() const
Definition: vpMeSite.h:238
Definition: vpMe.h:122
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
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:458
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:462
void projection(const vpColVector &_cP, vpColVector &_p) const
Definition: vpPoint.cpp:219
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:460
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:236