ViSP  2.10.0
vpMbtDistanceCylinder.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbtDistanceCylinder.cpp 4914 2014-10-02 13:25:47Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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  : name(), index(0), cam(), me(NULL), alpha(0), wmean1(1), wmean2(1),
68  featureline1(), featureline2(), meline1(NULL), meline2(NULL),
69  cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(),
70  error(), nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false),
71  c(NULL), hiddenface(NULL), index_polygon(-1), isvisible(false)
72 {
73 }
74 
78 vpMbtDistanceCylinder::~vpMbtDistanceCylinder()
79 {
80 // cout << "Deleting cylinder " << index << endl ;
81 
82  if (p1 != NULL) delete p1 ;
83  if (p2 != NULL) delete p2 ;
84  if (c != NULL) delete c ;
85  if (meline1 != NULL) delete meline1 ;
86  if (meline2 != NULL) delete meline2 ;
87  if (cercle1 != NULL) delete cercle1 ;
88  if (cercle2 != NULL) delete cercle2 ;
89 }
90 
96 void
97 vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
98 {
99  c->project(cMo) ;
100  p1->project(cMo) ;
101  p2->project(cMo) ;
102  cercle1->project(cMo) ;
103  cercle2->project(cMo) ;
104 }
105 
106 
114 void
115 vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const 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,(_p1.get_oY()+_p2.get_oY())/2.0,(_p1.get_oZ()+_p2.get_oZ())/2.0,r);
150 }
151 
152 
158 void
159 vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
160 {
161  me = _me ;
162  if (meline1 != NULL)
163  {
164  meline1->setMe(me) ;
165  }
166  if (meline2 != NULL)
167  {
168  meline2->setMe(me) ;
169  }
170 }
171 
182 void
183 vpMbtDistanceCylinder::getCylinderLineExtremity(double &i, double &j,double rho, double theta,
184  vpCircle *circle)
185 {
186 // This was taken from the code of art-v1. (from the artCylinder class)
187  double px = cam.get_px() ;
188  double py = cam.get_py() ;
189  double u0 = cam.get_u0() ;
190  double v0 = cam.get_v0() ;
191 
192  double mu11 = circle->p[3];
193  double mu02 = circle->p[4];
194  double mu20 = circle->p[2];
195  double Xg = u0 + circle->p[0]*px;
196  double Yg = v0 + circle->p[1]*py;
197 
198  // Find Intersection between line and ellipse in the image.
199 
200  // Optimised calculation for X
201  double stheta = sin(theta);
202  double ctheta = cos(theta);
203  double sctheta = stheta*ctheta;
204  double m11yg = mu11*Yg;
205  double ctheta2 = vpMath::sqr(ctheta);
206  double m02xg = mu02*Xg;
207  double m11stheta = mu11*stheta;
208  j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
209  -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
210  m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
211  +mu02-mu02*ctheta2));
212  //Optimised calculation for Y
213  double rhom02 = rho*mu02;
214  double sctheta2 = stheta*ctheta2;
215  double ctheta3 = ctheta2*ctheta;
216  i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
217  +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
218  +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
219  (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
220  mu02*ctheta2)/stheta);
221 
222 }
223 
224 
233 bool
234 vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
235 {
236  if(isvisible){
237  // Perspective projection
238  p1->changeFrame(cMo);
239  p2->changeFrame(cMo);
240  cercle1->changeFrame(cMo);
241  cercle2->changeFrame(cMo);
242  c->changeFrame(cMo);
243 
244  p1->projection();
245  p2->projection();
246  try{
247  cercle1->projection();
248  }
249  catch(...){
250  //std::cout<<"Problem when projecting circle 1\n";
251  return false;
252  }
253  try{
254  cercle2->projection();
255  }
256  catch(...){
257  //std::cout<<"Problem when projecting circle 2\n";
258  return false;
259  }
260  c->projection();
261 
262  double rho1,theta1;
263  double rho2,theta2;
264 
265  // Create the moving edges containers
266  meline1 = new vpMbtMeLine ;
267  meline1->setMe(me) ;
268  meline2 = new vpMbtMeLine ;
269  meline2->setMe(me) ;
270 
271  // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
272  meline1->setInitRange(0);
273  meline2->setInitRange(0);
274 
275  // Conversion meter to pixels
276  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
277  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
278 
279  // Determine intersections between circles and limbos
280  double i11,i12,i21,i22,j11,j12,j21,j22;
281  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
282  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
283  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
284  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
285 
286  // Create the image points
287  vpImagePoint ip11,ip12,ip21,ip22;
288  ip11.set_ij(i11,j11);
289  ip12.set_ij(i12,j12);
290  ip21.set_ij(i21,j21);
291  ip22.set_ij(i22,j22);
292 
293  // update limits of the melines.
294  int marge = /*10*/5; //ou 5 normalement
295  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 ; }
296  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 ; }
297 
298  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 ; }
299  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 ; }
300 
301  // Initialize the tracking
302  while (theta1 > M_PI) { theta1 -= M_PI ; }
303  while (theta1 < -M_PI) { theta1 += M_PI ; }
304 
305  if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
306  else theta1 = M_PI/2.0 - theta1;
307 
308  while (theta2 > M_PI) { theta2 -= M_PI ; }
309  while (theta2 < -M_PI) { theta2 += M_PI ; }
310 
311  if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
312  else theta2 = M_PI/2.0 - theta2;
313 
314  try
315  {
316  meline1->initTracking(I,ip11,ip12,rho1,theta1);
317  }
318  catch(...)
319  {
320  //vpTRACE("the line can't be initialized");
321  return false;
322  }
323  try
324  {
325  meline2->initTracking(I,ip21,ip22,rho2,theta2);
326  }
327  catch(...)
328  {
329  //vpTRACE("the line can't be initialized");
330  return false;
331  }
332  }
333  return true;
334 }
335 
336 
337 
344 void
345 vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
346 {
347  if(isvisible){
348  try
349  {
350  meline1->track(I) ;
351  }
352  catch(...)
353  {
354  //std::cout << "Track meline1 failed" << std::endl;
355  meline1->reset();
356  Reinit = true;
357  }
358  try
359  {
360  meline2->track(I) ;
361  }
362  catch(...)
363  {
364  //std::cout << "Track meline2 failed" << std::endl;
365  meline2->reset();
366  Reinit = true;
367  }
368 
369  // Update the number of features
370  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
371  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
372  nbFeature = nbFeaturel1 + nbFeaturel2;
373  }
374 }
375 
376 
383 void
384 vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
385 {
386  if(isvisible){
387  // Perspective projection
388  p1->changeFrame(cMo);
389  p2->changeFrame(cMo);
390  cercle1->changeFrame(cMo);
391  cercle2->changeFrame(cMo);
392  c->changeFrame(cMo);
393 
394  p1->projection();
395  p2->projection();
396  try{
397  cercle1->projection();
398  }
399  catch(...){std::cout<<"Probleme projection cercle 1\n";}
400  try{
401  cercle2->projection();
402  }
403  catch(...){std::cout<<"Probleme projection cercle 2\n";}
404  c->projection();
405 
406  // Get the limbos
407  double rho1,theta1;
408  double rho2,theta2;
409 
410  // Conversion meter to pixels
411  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
412  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
413 
414  // Determine intersections between circles and limbos
415  double i11,i12,i21,i22,j11,j12,j21,j22;
416 
417  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
418  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
419 
420  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
421  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
422 
423  // Create the image points
424  vpImagePoint ip11,ip12,ip21,ip22;
425  ip11.set_ij(i11,j11);
426  ip12.set_ij(i12,j12);
427  ip21.set_ij(i21,j21);
428  ip22.set_ij(i22,j22);
429 
430  // update limits of the meline.
431  int marge = /*10*/5; //ou 5 normalement
432  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 ; }
433  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 ; }
434 
435  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 ; }
436  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 ; }
437 
438  // Initialize the tracking
439  while (theta1 > M_PI) { theta1 -= M_PI ; }
440  while (theta1 < -M_PI) { theta1 += M_PI ; }
441 
442  if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
443  else theta1 = M_PI/2.0 - theta1;
444 
445  while (theta2 > M_PI) { theta2 -= M_PI ; }
446  while (theta2 < -M_PI) { theta2 += M_PI ; }
447 
448  if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
449  else theta2 = M_PI/2.0 - theta2;
450 
451  try
452  {
453  //meline1->updateParameters(I,rho1,theta1) ;
454  meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
455  }
456  catch(...)
457  {
458  Reinit = true;
459  }
460  try
461  {
462  //meline2->updateParameters(I,rho2,theta2) ;
463  meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
464  }
465  catch(...)
466  {
467  Reinit = true;
468  }
469 
470  // Update the numbers of features
471  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
472  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
473  nbFeature = nbFeaturel1 + nbFeaturel2;
474  }
475 }
476 
477 
486 void
487 vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
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)
498  Reinit = true;
499 
500  Reinit = false;
501 }
502 
503 
514 void
515 vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
516  const vpCameraParameters&camera, const vpColor col, const unsigned int thickness,
517  const bool displayFullModel)
518 {
519  if(isvisible || displayFullModel){
520  // Perspective projection
521  p1->changeFrame(cMo);
522  p2->changeFrame(cMo);
523  cercle1->changeFrame(cMo);
524  cercle2->changeFrame(cMo);
525  c->changeFrame(cMo);
526 
527  p1->projection();
528  p2->projection();
529  try{
530  cercle1->projection();
531  }
532  catch(...){std::cout<<"Problem projection circle 1";}
533  try{
534  cercle2->projection();
535  }
536  catch(...){std::cout<<"Problem projection circle 2";}
537  c->projection();
538 
539  double rho1,theta1;
540  double rho2,theta2;
541 
542  // Meters to pixels conversion
543  vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
544  vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
545 
546  // Determine intersections between circles and limbos
547  double i11,i12,i21,i22,j11,j12,j21,j22;
548 
549  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
550  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
551 
552  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
553  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
554 
555  // Create the image points
556  vpImagePoint ip11,ip12,ip21,ip22;
557  ip11.set_ij(i11,j11);
558  ip12.set_ij(i12,j12);
559  ip21.set_ij(i21,j21);
560  ip22.set_ij(i22,j22);
561 
562  // Display
563  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
564  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
565  }
566 }
567 
578 void
579 vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
580  const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
581  const bool displayFullModel)
582 {
583  if(isvisible || displayFullModel){
584  // Perspective projection
585  p1->changeFrame(cMo);
586  p2->changeFrame(cMo);
587  cercle1->changeFrame(cMo);
588  cercle2->changeFrame(cMo);
589  c->changeFrame(cMo);
590 
591  p1->projection();
592  p2->projection();
593  try{
594  cercle1->projection();
595  }
596  catch(...){std::cout<<"Problem projection circle 1";}
597  try{
598  cercle2->projection();
599  }
600  catch(...){std::cout<<"Problem projection circle 2";}
601  c->projection();
602 
603  double rho1,theta1;
604  double rho2,theta2;
605 
606  // Meters to pixels conversion
607  vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
608  vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
609 
610  // Determine intersections between circles and limbos
611  double i11,i12,i21,i22,j11,j12,j21,j22;
612 
613  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
614  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
615 
616  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
617  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
618 
619  // Create the image points
620  vpImagePoint ip11,ip12,ip21,ip22;
621  ip11.set_ij(i11,j11);
622  ip12.set_ij(i12,j12);
623  ip21.set_ij(i21,j21);
624  ip22.set_ij(i22,j22);
625 
626  // Display
627  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
628  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
629  }
630 }
631 
632 
643 void
644 vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
645 {
646  if (meline1 != NULL)
647  {
648  meline1->display(I);
649  }
650  if (meline2 != NULL)
651  {
652  meline2->display(I);
653  }
654 }
655 
659 void
660 vpMbtDistanceCylinder::initInteractionMatrixError()
661 {
662  if (isvisible == true) {
663  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
664  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
665  nbFeature = nbFeaturel1 + nbFeaturel2;
666  L.resize(nbFeature, 6);
667  error.resize(nbFeature);
668  }
669  else {
670  nbFeature = 0 ;
671  nbFeaturel1 = 0;
672  nbFeaturel2 = 0;
673  }
674 }
675 
679 void
680 vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
681 {
682  if (isvisible) {
683  // Perspective projection
684  c->changeFrame(cMo) ;
685  c->projection() ;
686  cercle1->changeFrame(cMo) ;
687  cercle1->changeFrame(cMo) ;
688  try{
689  cercle1->projection();
690  }
691  catch(...){std::cout<<"Problem projection circle 1\n";}
692  try{
693  cercle2->projection();
694  }
695  catch(...){std::cout<<"Problem projection circle 2\n";}
696 
697  bool disp = false;
698  bool disp2 = false;
699  if (disp || disp2) vpDisplay::flush(I);
700 
701  // Build the lines
702  vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
703  vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
704 
705  double rho1 = featureline1.getRho() ;
706  double theta1 = featureline1.getTheta() ;
707  double rho2 = featureline2.getRho() ;
708  double theta2 = featureline2.getTheta() ;
709 
710  double co1 = cos(theta1);
711  double si1 = sin(theta1);
712  double co2 = cos(theta2);
713  double si2 = sin(theta2);
714 
715  double mx = 1.0/cam.get_px() ;
716  double my = 1.0/cam.get_py() ;
717  double xc = cam.get_u0() ;
718  double yc = cam.get_v0() ;
719 
720  double alpha1 ;
721  vpMatrix H1 ;
722  H1 = featureline1.interaction() ;
723  double alpha2 ;
724  vpMatrix H2 ;
725  H2 = featureline2.interaction() ;
726 
727  double x,y ;
728  vpMeSite p ;
729  unsigned int j =0 ;
730  for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
731  x = (double)it->j;
732  y = (double)it->i;
733 
734  x = (x-xc)*mx ;
735  y = (y-yc)*my ;
736 
737  alpha1 = x*si1 - y*co1;
738 
739  double *Lrho = H1[0] ;
740  double *Ltheta = H1[1] ;
741  // Calculate interaction matrix for a distance
742  for (unsigned int k=0 ; k < 6 ; k++){
743  L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
744  }
745  error[j] = rho1 - ( x*co1 + y*si1) ;
746 
747  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
748 
749  j++;
750  }
751 
752  for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
753  x = (double)it->j;
754  y = (double)it->i;
755 
756  x = (x-xc)*mx ;
757  y = (y-yc)*my ;
758 
759  alpha2 = x*si2 - y*co2;
760 
761  double *Lrho = H2[0] ;
762  double *Ltheta = H2[1] ;
763  // Calculate interaction matrix for a distance
764  for (unsigned int k=0 ; k < 6 ; k++){
765  L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
766  }
767  error[j] = rho2 - ( x*co2 + y*si2) ;
768 
769  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
770 
771  j++;
772  }
773  }
774 }
775 
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
vpMbtDistanceCylinder::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:644
vpMbtDistanceCylinder::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:384
vpMbtDistanceCylinder::c
vpCylinder * c
The cylinder.
Definition: vpMbtDistanceCylinder.h:113
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:296
vpMatrix::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
vpMbtDistanceCylinder::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:345
vpImagePoint::get_i
double get_i() const
Definition: vpImagePoint.h:195
vpCircle::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:273
vpMbtDistanceCylinder::isvisible
bool isvisible
Indicates if the cylinder is visible or not.
Definition: vpMbtDistanceCylinder.h:120
vpHomogeneousMatrix
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Definition: vpHomogeneousMatrix.h:103
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:125
vpPoint::get_oY
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
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:515
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:105
vpFeatureLine::getRho
double getRho() const
Definition: vpFeatureLine.h:235
vpCameraParameters::get_py
double get_py() const
Definition: vpCameraParameters.h:295
vpFeatureLine::getTheta
double getTheta() const
Definition: vpFeatureLine.h:240
vpMbtDistanceCylinder::p1
vpPoint * p1
The first extremity on the axe.
Definition: vpMbtDistanceCylinder.h:96
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
vpImagePoint::get_j
double get_j() const
Definition: vpImagePoint.h:206
vpCylinder::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:302
vpColor::red
static const vpColor red
Definition: vpColor.h:167
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:65
vpMbtDistanceCylinder::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceCylinder.h:101
vpColor::orange
static const vpColor orange
Definition: vpColor.h:177
vpMbtDistanceCylinder::nbFeaturel1
unsigned int nbFeaturel1
The number of moving edges on line 1.
Definition: vpMbtDistanceCylinder.h:107
vpCylinder::getTheta1
double getTheta1() const
Definition: vpCylinder.h:141
vpCircle::projection
void projection()
Definition: vpCircle.cpp:159
vpMbtDistanceCylinder::cercle2
vpCircle * cercle2
The lower circle limiting the cylinder.
Definition: vpMbtDistanceCylinder.h:90
vpCameraParameters::get_v0
double get_v0() const
Definition: vpCameraParameters.h:297
vpMbtDistanceCylinder::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:487
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:106
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:210
vpPoint::get_oZ
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
vpMbtDistanceCylinder::initMovingEdge
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:234
vpMbtDistanceCylinder::meline1
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Definition: vpMbtDistanceCylinder.h:83
vpFeatureLine::interaction
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
Definition: vpFeatureLine.cpp:208
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:292
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:98
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::buildFrom
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
Definition: vpMbtDistanceCylinder.cpp:115
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:103
vpMbtDistanceCylinder::cercle1
vpCircle * cercle1
The upper circle limiting the cylinder.
Definition: vpMbtDistanceCylinder.h:88
vpMbtDistanceCylinder::Reinit
bool Reinit
Indicates if the line has to be reinitialized.
Definition: vpMbtDistanceCylinder.h:111
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
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:93
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::nbFeaturel2
unsigned int nbFeaturel2
The number of moving edges on line 2.
Definition: vpMbtDistanceCylinder.h:109
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:85
vpMbtDistanceCylinder::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:680
vpTracker::p
vpColVector p
Definition: vpTracker.h:78
vpImagePoint::set_ij
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:181
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:98