Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpMbtDistanceCylinder.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Make the complete tracking of an object by using its CAD model. Cylinder
32  * tracking.
33  *
34  * Authors:
35  * Nicolas Melchior
36  * Romain Tallonneau
37  * Eric Marchand
38  * Bertrand Delabarre
39  *
40  *****************************************************************************/
41 
42 #include <visp3/core/vpConfig.h>
43 
49 #include <visp3/mbt/vpMbtDistanceCylinder.h>
50 #include <visp3/core/vpPlane.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
53 #include <visp3/visual_features/vpFeatureBuilder.h>
54 #include <visp3/visual_features/vpFeatureEllipse.h>
55 #include <stdlib.h>
56 #include <algorithm>
57 
58 #include <visp3/vision/vpPose.h>
59 
63 vpMbtDistanceCylinder::vpMbtDistanceCylinder()
64  : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1),
65  featureline1(), featureline2(), isTrackedCylinder(true), meline1(NULL), meline2(NULL),
66  cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(),
67  error(), nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false),
68  c(NULL), hiddenface(NULL), index_polygon(-1), isvisible(false)
69 {
70 }
71 
75 vpMbtDistanceCylinder::~vpMbtDistanceCylinder()
76 {
77 // cout << "Deleting cylinder " << index << endl ;
78 
79  if (p1 != NULL) delete p1 ;
80  if (p2 != NULL) delete p2 ;
81  if (c != NULL) delete c ;
82  if (meline1 != NULL) delete meline1 ;
83  if (meline2 != NULL) delete meline2 ;
84  if (cercle1 != NULL) delete cercle1 ;
85  if (cercle2 != NULL) delete cercle2 ;
86 }
87 
93 void
94 vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
95 {
96  c->project(cMo) ;
97  p1->project(cMo) ;
98  p2->project(cMo) ;
99  cercle1->project(cMo) ;
100  cercle2->project(cMo) ;
101 }
102 
103 
111 void
112 vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
113 {
114  c = new vpCylinder ;
115  p1 = new vpPoint ;
116  p2 = new vpPoint ;
117  cercle1 = new vpCircle;
118  cercle2 = new vpCircle;
119 
120  // Get the points
121  *p1 = _p1;
122  *p2 = _p2;
123 
124  // Get the radius
125  radius = r;
126 
127  vpColVector ABC(3);
128  vpColVector V1(3);
129  vpColVector V2(3);
130 
131  V1[0] = _p1.get_oX();
132  V1[1] = _p1.get_oY();
133  V1[2] = _p1.get_oZ();
134  V2[0] = _p2.get_oX();
135  V2[1] = _p2.get_oY();
136  V2[2] = _p2.get_oZ();
137 
138  // Get the axis of the cylinder
139  ABC = V1-V2;
140 
141  // Build our extremity circles
142  cercle1->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p1.get_oX(),_p1.get_oY(),_p1.get_oZ(),r);
143  cercle2->setWorldCoordinates(ABC[0],ABC[1],ABC[2],_p2.get_oX(),_p2.get_oY(),_p2.get_oZ(),r);
144 
145  // Build our cylinder
146  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);
147 }
148 
149 
155 void
156 vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
157 {
158  me = _me ;
159  if (meline1 != NULL)
160  {
161  meline1->setMe(me) ;
162  }
163  if (meline2 != NULL)
164  {
165  meline2->setMe(me) ;
166  }
167 }
168 
177 bool
178 vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
179 {
180  if(isvisible){
181  // Perspective projection
182  p1->changeFrame(cMo);
183  p2->changeFrame(cMo);
184  cercle1->changeFrame(cMo);
185  cercle2->changeFrame(cMo);
186  c->changeFrame(cMo);
187 
188  p1->projection();
189  p2->projection();
190  try{
191  cercle1->projection();
192  }
193  catch(...){
194  //std::cout<<"Problem when projecting circle 1\n";
195  return false;
196  }
197  try{
198  cercle2->projection();
199  }
200  catch(...){
201  //std::cout<<"Problem when projecting circle 2\n";
202  return false;
203  }
204  c->projection();
205 
206  double rho1,theta1;
207  double rho2,theta2;
208 
209  // Create the moving edges containers
210  meline1 = new vpMbtMeLine ;
211  meline1->setMe(me) ;
212  meline2 = new vpMbtMeLine ;
213  meline2->setMe(me) ;
214 
215  // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
216  meline1->setInitRange(0);
217  meline2->setInitRange(0);
218 
219  // Conversion meter to pixels
220  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
221  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
222 
223  // Determine intersections between circles and limbos
224  double i11,i12,i21,i22,j11,j12,j21,j22;
225  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
226  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
227  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
228  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
229 
230  // Create the image points
231  vpImagePoint ip11,ip12,ip21,ip22;
232  ip11.set_ij(i11,j11);
233  ip12.set_ij(i12,j12);
234  ip21.set_ij(i21,j21);
235  ip22.set_ij(i22,j22);
236 
237  // update limits of the melines.
238  int marge = /*10*/5; //ou 5 normalement
239  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 ; }
240  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 ; }
241 
242  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 ; }
243  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 ; }
244 
245  // Initialize the tracking
246  while (theta1 > M_PI) { theta1 -= M_PI ; }
247  while (theta1 < -M_PI) { theta1 += M_PI ; }
248 
249  if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
250  else theta1 = M_PI/2.0 - theta1;
251 
252  while (theta2 > M_PI) { theta2 -= M_PI ; }
253  while (theta2 < -M_PI) { theta2 += M_PI ; }
254 
255  if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
256  else theta2 = M_PI/2.0 - theta2;
257 
258  try
259  {
260  meline1->initTracking(I,ip11,ip12,rho1,theta1);
261  }
262  catch(...)
263  {
264  //vpTRACE("the line can't be initialized");
265  return false;
266  }
267  try
268  {
269  meline2->initTracking(I,ip21,ip22,rho2,theta2);
270  }
271  catch(...)
272  {
273  //vpTRACE("the line can't be initialized");
274  return false;
275  }
276  }
277  return true;
278 }
279 
280 
281 
288 void
289 vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
290 {
291  if(isvisible){
292  try
293  {
294  meline1->track(I) ;
295  }
296  catch(...)
297  {
298  //std::cout << "Track meline1 failed" << std::endl;
299  meline1->reset();
300  Reinit = true;
301  }
302  try
303  {
304  meline2->track(I) ;
305  }
306  catch(...)
307  {
308  //std::cout << "Track meline2 failed" << std::endl;
309  meline2->reset();
310  Reinit = true;
311  }
312 
313  // Update the number of features
314  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
315  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
316  nbFeature = nbFeaturel1 + nbFeaturel2;
317  }
318 }
319 
320 
327 void
328 vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
329 {
330  if(isvisible){
331  // Perspective projection
332  p1->changeFrame(cMo);
333  p2->changeFrame(cMo);
334  cercle1->changeFrame(cMo);
335  cercle2->changeFrame(cMo);
336  c->changeFrame(cMo);
337 
338  p1->projection();
339  p2->projection();
340  try{
341  cercle1->projection();
342  }
343  catch(...){std::cout<<"Probleme projection cercle 1\n";}
344  try{
345  cercle2->projection();
346  }
347  catch(...){std::cout<<"Probleme projection cercle 2\n";}
348  c->projection();
349 
350  // Get the limbos
351  double rho1,theta1;
352  double rho2,theta2;
353 
354  // Conversion meter to pixels
355  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
356  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
357 
358  // Determine intersections between circles and limbos
359  double i11,i12,i21,i22,j11,j12,j21,j22;
360 
361  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
362  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
363 
364  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
365  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
366 
367  // Create the image points
368  vpImagePoint ip11,ip12,ip21,ip22;
369  ip11.set_ij(i11,j11);
370  ip12.set_ij(i12,j12);
371  ip21.set_ij(i21,j21);
372  ip22.set_ij(i22,j22);
373 
374  // update limits of the meline.
375  int marge = /*10*/5; //ou 5 normalement
376  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 ; }
377  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 ; }
378 
379  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 ; }
380  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 ; }
381 
382  // Initialize the tracking
383  while (theta1 > M_PI) { theta1 -= M_PI ; }
384  while (theta1 < -M_PI) { theta1 += M_PI ; }
385 
386  if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
387  else theta1 = M_PI/2.0 - theta1;
388 
389  while (theta2 > M_PI) { theta2 -= M_PI ; }
390  while (theta2 < -M_PI) { theta2 += M_PI ; }
391 
392  if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
393  else theta2 = M_PI/2.0 - theta2;
394 
395  try
396  {
397  //meline1->updateParameters(I,rho1,theta1) ;
398  meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
399  }
400  catch(...)
401  {
402  Reinit = true;
403  }
404  try
405  {
406  //meline2->updateParameters(I,rho2,theta2) ;
407  meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
408  }
409  catch(...)
410  {
411  Reinit = true;
412  }
413 
414  // Update the numbers of features
415  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
416  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
417  nbFeature = nbFeaturel1 + nbFeaturel2;
418  }
419 }
420 
421 
430 void
431 vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
432 {
433  if(meline1!= NULL)
434  delete meline1;
435  if(meline2!= NULL)
436  delete meline2;
437 
438  meline1 = NULL;
439  meline2 = NULL;
440 
441  if (initMovingEdge(I,cMo) == false)
442  Reinit = true;
443 
444  Reinit = false;
445 }
446 
447 
458 void
459 vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
460  const vpCameraParameters&camera, const vpColor col, const unsigned int thickness,
461  const bool displayFullModel)
462 {
463  if( (isvisible && isTrackedCylinder) || displayFullModel){
464  // Perspective projection
465  p1->changeFrame(cMo);
466  p2->changeFrame(cMo);
467  cercle1->changeFrame(cMo);
468  cercle2->changeFrame(cMo);
469  c->changeFrame(cMo);
470 
471  p1->projection();
472  p2->projection();
473  try{
474  cercle1->projection();
475  }
476  catch(...){std::cout<<"Problem projection circle 1";}
477  try{
478  cercle2->projection();
479  }
480  catch(...){std::cout<<"Problem projection circle 2";}
481  c->projection();
482 
483  double rho1,theta1;
484  double rho2,theta2;
485 
486  // Meters to pixels conversion
487  vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
488  vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
489 
490  // Determine intersections between circles and limbos
491  double i11,i12,i21,i22,j11,j12,j21,j22;
492 
493  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
494  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
495 
496  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
497  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
498 
499  // Create the image points
500  vpImagePoint ip11,ip12,ip21,ip22;
501  ip11.set_ij(i11,j11);
502  ip12.set_ij(i12,j12);
503  ip21.set_ij(i21,j21);
504  ip22.set_ij(i22,j22);
505 
506  // Display
507  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
508  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
509  }
510 }
511 
522 void
523 vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
524  const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
525  const bool displayFullModel)
526 {
527  if( (isvisible && isTrackedCylinder) || displayFullModel){
528  // Perspective projection
529  p1->changeFrame(cMo);
530  p2->changeFrame(cMo);
531  cercle1->changeFrame(cMo);
532  cercle2->changeFrame(cMo);
533  c->changeFrame(cMo);
534 
535  p1->projection();
536  p2->projection();
537  try{
538  cercle1->projection();
539  }
540  catch(...){std::cout<<"Problem projection circle 1";}
541  try{
542  cercle2->projection();
543  }
544  catch(...){std::cout<<"Problem projection circle 2";}
545  c->projection();
546 
547  double rho1,theta1;
548  double rho2,theta2;
549 
550  // Meters to pixels conversion
551  vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
552  vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
553 
554  // Determine intersections between circles and limbos
555  double i11,i12,i21,i22,j11,j12,j21,j22;
556 
557  vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
558  vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
559 
560  vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
561  vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
562 
563  // Create the image points
564  vpImagePoint ip11,ip12,ip21,ip22;
565  ip11.set_ij(i11,j11);
566  ip12.set_ij(i12,j12);
567  ip21.set_ij(i21,j21);
568  ip22.set_ij(i22,j22);
569 
570  // Display
571  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
572  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
573  }
574 }
575 
576 
587 void
588 vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
589 {
590  if (meline1 != NULL)
591  {
592  meline1->display(I);
593  }
594  if (meline2 != NULL)
595  {
596  meline2->display(I);
597  }
598 }
599 
603 void
604 vpMbtDistanceCylinder::initInteractionMatrixError()
605 {
606  if (isvisible == true) {
607  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
608  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
609  nbFeature = nbFeaturel1 + nbFeaturel2;
610  L.resize(nbFeature, 6);
611  error.resize(nbFeature);
612  }
613  else {
614  nbFeature = 0 ;
615  nbFeaturel1 = 0;
616  nbFeaturel2 = 0;
617  }
618 }
619 
623 void
624 vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
625 {
626  if (isvisible) {
627  // Perspective projection
628  c->changeFrame(cMo) ;
629  c->projection() ;
630  cercle1->changeFrame(cMo) ;
631  cercle1->changeFrame(cMo) ;
632  try{
633  cercle1->projection();
634  }
635  catch(...){std::cout<<"Problem projection circle 1\n";}
636  try{
637  cercle2->projection();
638  }
639  catch(...){std::cout<<"Problem projection circle 2\n";}
640 
641  bool disp = false;
642  bool disp2 = false;
643  if (disp || disp2) vpDisplay::flush(I);
644 
645  // Build the lines
646  vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
647  vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
648 
649  double rho1 = featureline1.getRho() ;
650  double theta1 = featureline1.getTheta() ;
651  double rho2 = featureline2.getRho() ;
652  double theta2 = featureline2.getTheta() ;
653 
654  double co1 = cos(theta1);
655  double si1 = sin(theta1);
656  double co2 = cos(theta2);
657  double si2 = sin(theta2);
658 
659  double mx = 1.0/cam.get_px() ;
660  double my = 1.0/cam.get_py() ;
661  double xc = cam.get_u0() ;
662  double yc = cam.get_v0() ;
663 
664  vpMatrix H1 ;
665  H1 = featureline1.interaction() ;
666  vpMatrix H2 ;
667  H2 = featureline2.interaction() ;
668 
669  vpMeSite p ;
670  unsigned int j =0 ;
671  for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
672  double x = (double)it->j;
673  double y = (double)it->i;
674 
675  x = (x-xc)*mx ;
676  y = (y-yc)*my ;
677 
678  double alpha1 = x*si1 - y*co1;
679 
680  double *Lrho = H1[0] ;
681  double *Ltheta = H1[1] ;
682  // Calculate interaction matrix for a distance
683  for (unsigned int k=0 ; k < 6 ; k++){
684  L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
685  }
686  error[j] = rho1 - ( x*co1 + y*si1) ;
687 
688  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
689 
690  j++;
691  }
692 
693  for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
694  double x = (double)it->j;
695  double y = (double)it->i;
696 
697  x = (x-xc)*mx ;
698  y = (y-yc)*my ;
699 
700  double alpha2 = x*si2 - y*co2;
701 
702  double *Lrho = H2[0] ;
703  double *Ltheta = H2[1] ;
704  // Calculate interaction matrix for a distance
705  for (unsigned int k=0 ; k < 6 ; k++){
706  L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
707  }
708  error[j] = rho2 - ( x*co2 + y*si2) ;
709 
710  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
711 
712  j++;
713  }
714  }
715 }
716 
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
vpMbtDistanceCylinder::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:588
vpMbtDistanceCylinder::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:328
vpMbtDistanceCylinder::c
vpCylinder * c
The cylinder.
Definition: vpMbtDistanceCylinder.h:110
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:229
vpCameraParameters::get_u0
double get_u0() const
Definition: vpCameraParameters.h:307
vpMbtDistanceCylinder::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:289
vpImagePoint::get_i
double get_i() const
Definition: vpImagePoint.h:199
vpCircle::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:269
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
vpMbtDistanceCylinder::isvisible
bool isvisible
Indicates if the cylinder is visible or not.
Definition: vpMbtDistanceCylinder.h:117
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:93
vpMeSite
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
Definition: vpMeSite.h:72
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
vpPoint::get_oY
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
vpMbtDistanceCylinder::display
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)
Definition: vpMbtDistanceCylinder.cpp:459
vpCylinder::getRho2
double getRho2() const
Definition: vpCylinder.h:146
vpMe
Definition: vpMe.h:59
vpMbtDistanceCylinder::nbFeature
unsigned int nbFeature
The number of moving edges.
Definition: vpMbtDistanceCylinder.h:102
vpFeatureLine::getRho
double getRho() const
Definition: vpFeatureLine.h:239
vpCameraParameters::get_py
double get_py() const
Definition: vpCameraParameters.h:306
vpFeatureLine::getTheta
double getTheta() const
Definition: vpFeatureLine.h:244
vpMbtDistanceCylinder::p1
vpPoint * p1
The first extremity on the axe.
Definition: vpMbtDistanceCylinder.h:93
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:593
vpImagePoint::get_j
double get_j() const
Definition: vpImagePoint.h:210
vpCylinder::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:298
vpColor::red
static const vpColor red
Definition: vpColor.h:163
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:59
vpMbtDistanceCylinder::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceCylinder.h:98
vpColor::orange
static const vpColor orange
Definition: vpColor.h:173
vpMbtDistanceCylinder::nbFeaturel1
unsigned int nbFeaturel1
The number of moving edges on line 1.
Definition: vpMbtDistanceCylinder.h:104
vpCylinder::getTheta1
double getTheta1() const
Definition: vpCylinder.h:139
vpCircle::projection
void projection()
Definition: vpCircle.cpp:155
vpMbtDistanceCylinder::cercle2
vpCircle * cercle2
The lower circle limiting the cylinder.
Definition: vpMbtDistanceCylinder.h:87
vpCameraParameters::get_v0
double get_v0() const
Definition: vpCameraParameters.h:308
vpMbtDistanceCylinder::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:431
vpCircle::computeIntersectionPoint
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:373
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:221
vpPoint::get_oZ
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:451
vpMbtDistanceCylinder::initMovingEdge
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:178
vpMbtDistanceCylinder::meline1
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Definition: vpMbtDistanceCylinder.h:80
vpFeatureLine::interaction
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
Definition: vpFeatureLine.cpp:203
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:303
vpCylinder::projection
void projection()
Definition: vpCylinder.cpp:185
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:52
vpMbtDistanceCylinder::p2
vpPoint * p2
The second extremity on the axe.
Definition: vpMbtDistanceCylinder.h:95
vpPoint::get_oX
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
vpDisplay::displayCross
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:152
vpCylinder
Class that defines what is a cylinder.
Definition: vpCylinder.h:93
vpMeSite::j
int j
Definition: vpMeSite.h:94
vpMbtDistanceCylinder::buildFrom
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
Definition: vpMbtDistanceCylinder.cpp:112
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpCylinder::getRho1
double getRho1() const
Definition: vpCylinder.h:133
vpCylinder::getTheta2
double getTheta2() const
Definition: vpCylinder.h:152
vpMbtDistanceCylinder::error
vpColVector error
The error vector.
Definition: vpMbtDistanceCylinder.h:100
vpMbtDistanceCylinder::cercle1
vpCircle * cercle1
The upper circle limiting the cylinder.
Definition: vpMbtDistanceCylinder.h:85
vpMbtDistanceCylinder::Reinit
bool Reinit
Indicates if the line has to be reinitialized.
Definition: vpMbtDistanceCylinder.h:108
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
vpMbtDistanceCylinder::radius
double radius
The radius of the cylinder.
Definition: vpMbtDistanceCylinder.h:90
vpDisplay::displayLine
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:337
vpCylinder::setWorldCoordinates
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:73
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:89
vpMbtDistanceCylinder::nbFeaturel2
unsigned int nbFeaturel2
The number of moving edges on line 2.
Definition: vpMbtDistanceCylinder.h:106
vpPoint::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:247
vpCircle
Class that defines what is a circle.
Definition: vpCircle.h:57
vpMbtDistanceCylinder::meline2
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
Definition: vpMbtDistanceCylinder.h:82
vpMbtDistanceCylinder::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:624
vpImagePoint::set_ij
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:185
vpCircle::setWorldCoordinates
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:62
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225