ViSP  2.7.0
vpMbtDistanceCylinder.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbtDistanceCylinder.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Make the complete tracking of an object by using its CAD model. Cylinder
35  * tracking.
36  *
37  * Authors:
38  * Nicolas Melchior
39  * Romain Tallonneau
40  * Eric Marchand
41  * Bertrand Delabarre
42  *
43  *****************************************************************************/
44 
45 #include <visp/vpConfig.h>
46 
52 #include <visp/vpMbtDistanceCylinder.h>
53 #include <visp/vpPlane.h>
54 #include <visp/vpMeterPixelConversion.h>
55 #include <visp/vpPixelMeterConversion.h>
56 #include <visp/vpFeatureBuilder.h>
57 #include <visp/vpFeatureEllipse.h>
58 #include <stdlib.h>
59 #include <algorithm>
60 
61 #include <visp/vpPose.h>
62 
66 vpMbtDistanceCylinder::vpMbtDistanceCylinder()
67 {
68  name = "";
69  p1 = NULL ;
70  p2 = NULL ;
71  c = NULL ;
72  meline1 = NULL ;
73  meline2 = NULL ;
74  wmean1 = 1 ;
75  wmean2 = 1 ;
76  nbFeaturel1 =0 ;
77  nbFeaturel2 =0 ;
78  nbFeature =0 ;
79  Reinit = false;
80 
81  cercle1 = NULL;
82  cercle2 = NULL;
83 }
84 
88 vpMbtDistanceCylinder::~vpMbtDistanceCylinder()
89 {
90 // cout << "Deleting cylinder " << index << endl ;
91 
92  if (p1 != NULL) delete p1 ;
93  if (p2 != NULL) delete p2 ;
94  if (c != NULL) delete c ;
95  if (meline1 != NULL) delete meline1 ;
96  if (meline2 != NULL) delete meline2 ;
97  if (cercle1 != NULL) delete cercle1 ;
98  if (cercle2 != NULL) delete cercle2 ;
99 }
100 
106 void
107 vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
108 {
109  c->project(cMo) ;
110  p1->project(cMo) ;
111  p2->project(cMo) ;
112  cercle1->project(cMo) ;
113  cercle2->project(cMo) ;
114 }
115 
116 
124 void
125 vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
126 {
127  c = new vpCylinder ;
128  p1 = new vpPoint ;
129  p2 = new vpPoint ;
130  cercle1 = new vpCircle;
131  cercle2 = new vpCircle;
132 
133  // Get the points
134  *p1 = _p1;
135  *p2 = _p2;
136 
137  // Get the radius
138  radius = r;
139 
140  vpColVector ABC(3);
141  vpColVector V1(3);
142  vpColVector V2(3);
143 
144  V1[0] = _p1.get_oX();
145  V1[1] = _p1.get_oY();
146  V1[2] = _p1.get_oZ();
147  V2[0] = _p2.get_oX();
148  V2[1] = _p2.get_oY();
149  V2[2] = _p2.get_oZ();
150 
151  // Get the axis of the cylinder
152  ABC = V1-V2;
153 
154  // Build our extremity circles
155  cercle1->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p1.get_oX(),_p1.get_oY(),_p1.get_oZ(),r);
156  cercle2->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p2.get_oX(),_p2.get_oY(),_p2.get_oZ(),r);
157 
158  // Build our cylinder
159  c->setWorldCoordinates(ABC[0],ABC[1],ABC[2],(_p1.get_oX()+_p2.get_oX())/2.0,(_p1.get_oY()+_p2.get_oY())/2.0,(_p1.get_oZ()+_p2.get_oZ())/2.0,r);
160 }
161 
162 
168 void
169 vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
170 {
171  me = _me ;
172  if (meline1 != NULL)
173  {
174  meline1->setMe(me) ;
175  }
176  if (meline2 != NULL)
177  {
178  meline2->setMe(me) ;
179  }
180 }
181 
192 void
193 vpMbtDistanceCylinder::getCylinderLineExtremity(double &i, double &j,double rho, double theta,
194  vpCircle *circle)
195 {
196 // This was taken from the code of art-v1. (from the artCylinder class)
197  double px = cam.get_px() ;
198  double py = cam.get_py() ;
199  double u0 = cam.get_u0() ;
200  double v0 = cam.get_v0() ;
201 
202  double mu11 = circle->p[3];
203  double mu02 = circle->p[4];
204  double mu20 = circle->p[2];
205  double Xg = u0 + circle->p[0]*px;
206  double Yg = v0 + circle->p[1]*py;
207 
208  // Find Intersection between line and ellipse in the image.
209 
210  // Optimised calculation for X
211  double stheta = sin(theta);
212  double ctheta = cos(theta);
213  double sctheta = stheta*ctheta;
214  double m11yg = mu11*Yg;
215  double ctheta2 = vpMath::sqr(ctheta);
216  double m02xg = mu02*Xg;
217  double m11stheta = mu11*stheta;
218  j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
219  -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
220  m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
221  +mu02-mu02*ctheta2));
222  //Optimised calculation for Y
223  double rhom02 = rho*mu02;
224  double sctheta2 = stheta*ctheta2;
225  double ctheta3 = ctheta2*ctheta;
226  i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
227  +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
228  +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
229  (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
230  mu02*ctheta2)/stheta);
231 
232 }
233 
234 
242 void
243 vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
244 {
245  // Perspective projection
246  p1->changeFrame(cMo);
247  p2->changeFrame(cMo);
248  cercle1->changeFrame(cMo);
249  cercle2->changeFrame(cMo);
250  c->changeFrame(cMo);
251 
252  p1->projection();
253  p2->projection();
254  try{
255  cercle1->projection();
256  }
257  catch(...){std::cout<<"Problem when projecting circle 1\n";}
258  try{
259  cercle2->projection();
260  }
261  catch(...){std::cout<<"Problem when projecting circle 2\n";}
262  c->projection();
263 
264  double rho1,theta1;
265  double rho2,theta2;
266 
267 // Create the moving edges containers
268  meline1 = new vpMbtMeLine ;
269  meline1->setMe(me) ;
270  meline2 = new vpMbtMeLine ;
271  meline2->setMe(me) ;
272 
273 // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
274  meline1->setInitRange(0);
275  meline2->setInitRange(0);
276 
277  // Conversion meter to pixels
278  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
279  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
280 
281  // Determine intersections between circles and limbos
282  double i11,i12,i21,i22,j11,j12,j21,j22;
283  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
284  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
285  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
286  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
287 
288  // Create the image points
289  vpImagePoint ip11,ip12,ip21,ip22;
290  ip11.set_ij(i11,j11);
291  ip12.set_ij(i12,j12);
292  ip21.set_ij(i21,j21);
293  ip22.set_ij(i22,j22);
294 
295  // update limits of the melines.
296  int marge = /*10*/5; //ou 5 normalement
297  if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
298  if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
299 
300  if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
301  if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
302 
303  // Initialize the tracking
304  try
305  {
306  meline1->initTracking(I,ip11,ip12,rho1,theta1);
307  }
308  catch(...)
309  {
310  vpTRACE("the line can't be initialized");
311  }
312  try
313  {
314  meline2->initTracking(I,ip21,ip22,rho2,theta2);
315  }
316  catch(...)
317  {
318  vpTRACE("the line can't be initialized");
319  }
320 }
321 
322 
323 
330 void
331 vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
332 {
333  try
334  {
335  meline1->track(I) ;
336  }
337  catch(...)
338  {
339  std::cout << "Track meline1 failed" << std::endl;
340  Reinit = true;
341  }
342  try
343  {
344  meline2->track(I) ;
345  }
346  catch(...)
347  {
348  std::cout << "Track meline2 failed" << std::endl;
349  Reinit = true;
350  }
351 
352  // Update the number of features
353  nbFeaturel1 = meline1->getMeList().size();
354  nbFeaturel2 = meline2->getMeList().size();
355  nbFeature = meline1->getMeList().size()+meline2->getMeList().size();
356 }
357 
358 
365 void
366 vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
367 {
368  // Perspective projection
369  p1->changeFrame(cMo);
370  p2->changeFrame(cMo);
371  cercle1->changeFrame(cMo);
372  cercle2->changeFrame(cMo);
373  c->changeFrame(cMo);
374 
375  p1->projection();
376  p2->projection();
377  try{
378  cercle1->projection();
379  }
380  catch(...){std::cout<<"Probleme projection cercle 1\n";}
381  try{
382  cercle2->projection();
383  }
384  catch(...){std::cout<<"Probleme projection cercle 2\n";}
385  c->projection();
386 
387  // Get the limbos
388  double rho1,theta1;
389  double rho2,theta2;
390 
391  // Conversion meter to pixels
392  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
393  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
394 
395  // Determine intersections between circles and limbos
396  double i11,i12,i21,i22,j11,j12,j21,j22;
397 
398  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
399  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
400 
401  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
402  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
403 
404  // Create the image points
405  vpImagePoint ip11,ip12,ip21,ip22;
406  ip11.set_ij(i11,j11);
407  ip12.set_ij(i12,j12);
408  ip21.set_ij(i21,j21);
409  ip22.set_ij(i22,j22);
410 
411  // update limits of the meline.
412  int marge = /*10*/5; //ou 5 normalement
413  if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
414  if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
415 
416  if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
417  if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
418 
419  try
420  {
421  //meline1->updateParameters(I,rho1,theta1) ;
422  meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
423  }
424  catch(...)
425  {
426  Reinit = true;
427  }
428  try
429  {
430  //meline2->updateParameters(I,rho2,theta2) ;
431  meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
432  }
433  catch(...)
434  {
435  Reinit = true;
436  }
437 
438  // Update the numbers of features
439  nbFeaturel1 = meline1->getMeList().size();
440  nbFeaturel2 = meline2->getMeList().size();
441  nbFeature = meline1->getMeList().size()+meline2->getMeList().size();
442 }
443 
444 
453 void
454 vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
455 {
456  if(meline1!= NULL)
457  delete meline1;
458  if(meline2!= NULL)
459  delete meline2;
460 
461  meline1 = NULL;
462  meline2 = NULL;
463 
464  initMovingEdge(I,cMo);
465 
466  Reinit = false;
467 }
468 
469 
479 void
480 vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo, const vpCameraParameters&cam, const vpColor col, const unsigned int thickness)
481 {
482  // Perspective projection
483  p1->changeFrame(cMo);
484  p2->changeFrame(cMo);
485  cercle1->changeFrame(cMo);
486  cercle2->changeFrame(cMo);
487  c->changeFrame(cMo);
488 
489  p1->projection();
490  p2->projection();
491  try{
492  cercle1->projection();
493  }
494  catch(...){std::cout<<"Problem projection circle 1";}
495  try{
496  cercle2->projection();
497  }
498  catch(...){std::cout<<"Problem projection circle 2";}
499  c->projection();
500 
501  double rho1,theta1;
502  double rho2,theta2;
503 
504  // Meters to pixels conversion
505  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
506  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
507 
508  // Determine intersections between circles and limbos
509  double i11,i12,i21,i22,j11,j12,j21,j22;
510 
511  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
512  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
513 
514  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
515  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
516 
517  // Create the image points
518  vpImagePoint ip11,ip12,ip21,ip22;
519  ip11.set_ij(i11,j11);
520  ip12.set_ij(i12,j12);
521  ip21.set_ij(i21,j21);
522  ip22.set_ij(i22,j22);
523 
524  // Display
525  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
526  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
527 }
528 
538 void
539 vpMbtDistanceCylinder::display(const vpImage<vpRGBa>&I, const vpHomogeneousMatrix &cMo, const vpCameraParameters&cam, const vpColor col, const unsigned int thickness)
540 {
541  // Perspective projection
542  p1->changeFrame(cMo);
543  p2->changeFrame(cMo);
544  cercle1->changeFrame(cMo);
545  cercle2->changeFrame(cMo);
546  c->changeFrame(cMo);
547 
548  p1->projection();
549  p2->projection();
550  try{
551  cercle1->projection();
552  }
553  catch(...){std::cout<<"Problem projection circle 1";}
554  try{
555  cercle2->projection();
556  }
557  catch(...){std::cout<<"Problem projection circle 2";}
558  c->projection();
559 
560  double rho1,theta1;
561  double rho2,theta2;
562 
563  // Meters to pixels conversion
564  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
565  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
566 
567  // Determine intersections between circles and limbos
568  double i11,i12,i21,i22,j11,j12,j21,j22;
569 
570  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
571  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
572 
573  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
574  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
575 
576  // Create the image points
577  vpImagePoint ip11,ip12,ip21,ip22;
578  ip11.set_ij(i11,j11);
579  ip12.set_ij(i12,j12);
580  ip21.set_ij(i21,j21);
581  ip22.set_ij(i22,j22);
582 
583  // Display
584  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
585  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
586 }
587 
588 
599 void
600 vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
601 {
602  if (meline1 != NULL)
603  {
604  meline1->display(I);
605  }
606  if (meline2 != NULL)
607  {
608  meline2->display(I);
609  }
610 }
611 
615 void
616 vpMbtDistanceCylinder::initInteractionMatrixError()
617 {
618  L.resize(meline1->getMeList().size()+meline2->getMeList().size(),6) ;
619  error.resize(meline1->getMeList().size()+meline2->getMeList().size()) ;
620  nbFeaturel1 = meline1->getMeList().size();
621  nbFeaturel2 = meline2->getMeList().size();
622  nbFeature = meline1->getMeList().size()+meline2->getMeList().size() ;
623 }
624 
628 void
629 vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
630 {
631  // Perspective projection
632  c->changeFrame(cMo) ;
633  c->projection() ;
634  cercle1->changeFrame(cMo) ;
635  cercle1->changeFrame(cMo) ;
636  try{
637  cercle1->projection();
638  }
639  catch(...){std::cout<<"Problem projection circle 1\n";}
640  try{
641  cercle2->projection();
642  }
643  catch(...){std::cout<<"Problem projection circle 2\n";}
644 
645  bool disp = false;
646  bool disp2 = false;
647  if (disp || disp2) vpDisplay::flush(I);
648 
649  // Build the lines
650  vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
651  vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
652 
653  double rho1 = featureline1.getRho() ;
654  double theta1 = featureline1.getTheta() ;
655  double rho2 = featureline2.getRho() ;
656  double theta2 = featureline2.getTheta() ;
657 
658  double co1 = cos(theta1);
659  double si1 = sin(theta1);
660  double co2 = cos(theta2);
661  double si2 = sin(theta2);
662 
663  double mx = 1.0/cam.get_px() ;
664  double my = 1.0/cam.get_py() ;
665  double xc = cam.get_u0() ;
666  double yc = cam.get_v0() ;
667 
668  double alpha1 ;
669  vpMatrix H1 ;
670  H1 = featureline1.interaction() ;
671  double alpha2 ;
672  vpMatrix H2 ;
673  H2 = featureline2.interaction() ;
674 
675  double x,y ;
676  vpMeSite p ;
677  unsigned int j =0 ;
678  for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
679  x = (double)it->j;
680  y = (double)it->i;
681 
682  x = (x-xc)*mx ;
683  y = (y-yc)*my ;
684 
685  alpha1 = x*si1 - y*co1;
686 
687  double *Lrho = H1[0] ;
688  double *Ltheta = H1[1] ;
689  // Calculate interaction matrix for a distance
690  for (unsigned int k=0 ; k < 6 ; k++){
691  L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
692  }
693  error[j] = rho1 - ( x*co1 + y*si1) ;
694 
695  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
696 
697  j++;
698  }
699 
700  for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
701  x = (double)it->j;
702  y = (double)it->i;
703 
704  x = (x-xc)*mx ;
705  y = (y-yc)*my ;
706 
707  alpha2 = x*si2 - y*co2;
708 
709  double *Lrho = H2[0] ;
710  double *Ltheta = H2[1] ;
711  // Calculate interaction matrix for a distance
712  for (unsigned int k=0 ; k < 6 ; k++){
713  L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
714  }
715  error[j] = rho2 - ( x*co2 + y*si2) ;
716 
717  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
718 
719  j++;
720  }
721 }
722 
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpMbtDistanceCylinder::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:600
vpMbtDistanceCylinder::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:366
vpMbtDistanceCylinder::c
vpCylinder * c
The cylinder.
Definition: vpMbtDistanceCylinder.h:112
vpPoint::projection
void projection(const vpColVector &_cP, vpColVector &_p)
Projection onto the image plane of a point. Input: the 3D coordinates in the camera frame _cP...
Definition: vpPoint.cpp:132
vpCameraParameters::get_u0
double get_u0() const
Definition: vpCameraParameters.h:233
vpMatrix::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
vpMbtDistanceCylinder::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:331
vpImagePoint::get_i
double get_i() const
Definition: vpImagePoint.h:181
vpCircle::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:255
vpHomogeneousMatrix
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Definition: vpHomogeneousMatrix.h:102
vpTRACE
#define vpTRACE
Definition: vpDebug.h:401
vpMeSite
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:76
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:123
vpMbtMeLine::track
void track(const vpImage< unsigned char > &I)
vpPoint::get_oY
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
vpCylinder::getRho2
double getRho2() const
Definition: vpCylinder.h:148
vpMe
Contains predetermined masks for sites and holds moving edges tracking parameters.
Definition: vpMe.h:70
vpMbtDistanceCylinder::nbFeature
unsigned int nbFeature
The number of moving edges.
Definition: vpMbtDistanceCylinder.h:104
vpFeatureLine::getRho
double getRho() const
Definition: vpFeatureLine.h:235
vpCameraParameters::get_py
double get_py() const
Definition: vpCameraParameters.h:232
vpMbtMeLine
Implementation of a line used by the model-based tracker.
Definition: vpMbtMeLine.h:62
vpMbtMeLine::initTracking
void initTracking(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho, double theta)
vpFeatureLine::getTheta
double getTheta() const
Definition: vpFeatureLine.h:240
vpMbtDistanceCylinder::p1
vpPoint * p1
The first extremity on the axe.
Definition: vpMbtDistanceCylinder.h:95
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
vpImagePoint::get_j
double get_j() const
Definition: vpImagePoint.h:192
vpMbtMeLine::display
void display(const vpImage< unsigned char > &, vpColor)
Definition: vpMbtMeLine.h:80
vpCylinder::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:302
vpColor::red
static const vpColor red
Definition: vpColor.h:165
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:65
vpMbtDistanceCylinder::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceCylinder.h:100
vpColor::orange
static const vpColor orange
Definition: vpColor.h:175
vpMbtDistanceCylinder::nbFeaturel1
unsigned int nbFeaturel1
The number of moving edges on line 1.
Definition: vpMbtDistanceCylinder.h:106
vpCylinder::getTheta1
double getTheta1() const
Definition: vpCylinder.h:141
vpCircle::projection
void projection()
perspective projection of the circle
Definition: vpCircle.cpp:155
vpMbtMeLine::updateParameters
void updateParameters(const vpImage< unsigned char > &I, double rho, double theta)
vpMbtDistanceCylinder::cercle2
vpCircle * cercle2
The lower circle limiting the cylinder.
Definition: vpMbtDistanceCylinder.h:89
vpCameraParameters::get_v0
double get_v0() const
Definition: vpCameraParameters.h:234
vpMbtDistanceCylinder::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:454
vpImagePoint::set_ij
void set_ij(const double i, const double j)
Definition: vpImagePoint.h:167
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:106
vpMeTracker::setInitRange
void setInitRange(const unsigned int &r)
Definition: vpMeTracker.h:126
vpDisplay::displayCross
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:197
vpPoint::get_oZ
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
vpMbtDistanceCylinder::meline1
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Definition: vpMbtDistanceCylinder.h:82
vpFeatureLine::interaction
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
Definition: vpFeatureLine.cpp:208
vpMeTracker::getMeList
std::list< vpMeSite > & getMeList()
Definition: vpMeTracker.h:161
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:229
vpCylinder::projection
void projection()
Definition: vpCylinder.cpp:189
vpMeterPixelConversion::convertLine
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
line coordinates conversion (rho,theta)
Definition: vpMeterPixelConversion.cpp:56
vpMbtDistanceCylinder::p2
vpPoint * p2
The second extremity on the axe.
Definition: vpMbtDistanceCylinder.h:97
vpPoint::get_oX
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:127
vpCylinder
Class that defines what is a cylinder.
Definition: vpCylinder.h:97
vpMeSite::j
int j
Definition: vpMeSite.h:98
vpMbtDistanceCylinder::initMovingEdge
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:243
vpMbtDistanceCylinder::buildFrom
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
Definition: vpMbtDistanceCylinder.cpp:125
vpColVector
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpCylinder::getRho1
double getRho1() const
Definition: vpCylinder.h:135
vpCylinder::getTheta2
double getTheta2() const
Definition: vpCylinder.h:154
vpMbtDistanceCylinder::error
vpColVector error
The error vector.
Definition: vpMbtDistanceCylinder.h:102
vpMbtDistanceCylinder::cercle1
vpCircle * cercle1
The upper circle limiting the cylinder.
Definition: vpMbtDistanceCylinder.h:87
vpMbtDistanceCylinder::Reinit
bool Reinit
Indicates if the line has to be reinitialized.
Definition: vpMbtDistanceCylinder.h:110
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
vpDisplay::displayLine
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
vpMbtDistanceCylinder::radius
double radius
The radius of the cylinder.
Definition: vpMbtDistanceCylinder.h:92
vpCylinder::setWorldCoordinates
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:77
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:93
vpMbtDistanceCylinder::display
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness=1)
Definition: vpMbtDistanceCylinder.cpp:480
vpMbtDistanceCylinder::nbFeaturel2
unsigned int nbFeaturel2
The number of moving edges on line 2.
Definition: vpMbtDistanceCylinder.h:108
vpPoint::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:150
vpCircle
Class that defines what is a circle.
Definition: vpCircle.h:61
vpMbtDistanceCylinder::meline2
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
Definition: vpMbtDistanceCylinder.h:84
vpMbtDistanceCylinder::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:629
vpTracker::p
vpColVector p
Definition: vpTracker.h:78
vpMeTracker::setMe
void setMe(vpMe *me)
Definition: vpMeTracker.h:140
vpCircle::setWorldCoordinates
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:66
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94