ViSP  2.8.0
vpMbtDistanceCylinder.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbtDistanceCylinder.cpp 4303 2013-07-04 14:14:00Z 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  while (theta1 > M_PI) { theta1 -= M_PI ; }
305  while (theta1 < -M_PI) { theta1 += M_PI ; }
306 
307  if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
308  else theta1 = M_PI/2.0 - theta1;
309 
310  while (theta2 > M_PI) { theta2 -= M_PI ; }
311  while (theta2 < -M_PI) { theta2 += M_PI ; }
312 
313  if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
314  else theta2 = M_PI/2.0 - theta2;
315 
316  try
317  {
318  meline1->initTracking(I,ip11,ip12,rho1,theta1);
319  }
320  catch(...)
321  {
322  vpTRACE("the line can't be initialized");
323  }
324  try
325  {
326  meline2->initTracking(I,ip21,ip22,rho2,theta2);
327  }
328  catch(...)
329  {
330  vpTRACE("the line can't be initialized");
331  }
332 }
333 
334 
335 
342 void
343 vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
344 {
345  try
346  {
347  meline1->track(I) ;
348  }
349  catch(...)
350  {
351  std::cout << "Track meline1 failed" << std::endl;
352  Reinit = true;
353  }
354  try
355  {
356  meline2->track(I) ;
357  }
358  catch(...)
359  {
360  std::cout << "Track meline2 failed" << std::endl;
361  Reinit = true;
362  }
363 
364  // Update the number of features
365  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
366  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
367  nbFeature = nbFeaturel1 + nbFeaturel2;
368 }
369 
370 
377 void
378 vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
379 {
380  // Perspective projection
381  p1->changeFrame(cMo);
382  p2->changeFrame(cMo);
383  cercle1->changeFrame(cMo);
384  cercle2->changeFrame(cMo);
385  c->changeFrame(cMo);
386 
387  p1->projection();
388  p2->projection();
389  try{
390  cercle1->projection();
391  }
392  catch(...){std::cout<<"Probleme projection cercle 1\n";}
393  try{
394  cercle2->projection();
395  }
396  catch(...){std::cout<<"Probleme projection cercle 2\n";}
397  c->projection();
398 
399  // Get the limbos
400  double rho1,theta1;
401  double rho2,theta2;
402 
403  // Conversion meter to pixels
404  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
405  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
406 
407  // Determine intersections between circles and limbos
408  double i11,i12,i21,i22,j11,j12,j21,j22;
409 
410  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
411  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
412 
413  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
414  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
415 
416  // Create the image points
417  vpImagePoint ip11,ip12,ip21,ip22;
418  ip11.set_ij(i11,j11);
419  ip12.set_ij(i12,j12);
420  ip21.set_ij(i21,j21);
421  ip22.set_ij(i22,j22);
422 
423  // update limits of the meline.
424  int marge = /*10*/5; //ou 5 normalement
425  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 ; }
426  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 ; }
427 
428  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 ; }
429  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 ; }
430 
431  // Initialize the tracking
432  while (theta1 > M_PI) { theta1 -= M_PI ; }
433  while (theta1 < -M_PI) { theta1 += M_PI ; }
434 
435  if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
436  else theta1 = M_PI/2.0 - theta1;
437 
438  while (theta2 > M_PI) { theta2 -= M_PI ; }
439  while (theta2 < -M_PI) { theta2 += M_PI ; }
440 
441  if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
442  else theta2 = M_PI/2.0 - theta2;
443 
444  try
445  {
446  //meline1->updateParameters(I,rho1,theta1) ;
447  meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
448  }
449  catch(...)
450  {
451  Reinit = true;
452  }
453  try
454  {
455  //meline2->updateParameters(I,rho2,theta2) ;
456  meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
457  }
458  catch(...)
459  {
460  Reinit = true;
461  }
462 
463  // Update the numbers of features
464  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
465  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
466  nbFeature = nbFeaturel1 + nbFeaturel2;
467 }
468 
469 
478 void
479 vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
480 {
481  if(meline1!= NULL)
482  delete meline1;
483  if(meline2!= NULL)
484  delete meline2;
485 
486  meline1 = NULL;
487  meline2 = NULL;
488 
489  initMovingEdge(I,cMo);
490 
491  Reinit = false;
492 }
493 
494 
504 void
505 vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo, const vpCameraParameters&cam, const vpColor col, const unsigned int thickness)
506 {
507  // Perspective projection
508  p1->changeFrame(cMo);
509  p2->changeFrame(cMo);
510  cercle1->changeFrame(cMo);
511  cercle2->changeFrame(cMo);
512  c->changeFrame(cMo);
513 
514  p1->projection();
515  p2->projection();
516  try{
517  cercle1->projection();
518  }
519  catch(...){std::cout<<"Problem projection circle 1";}
520  try{
521  cercle2->projection();
522  }
523  catch(...){std::cout<<"Problem projection circle 2";}
524  c->projection();
525 
526  double rho1,theta1;
527  double rho2,theta2;
528 
529  // Meters to pixels conversion
530  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
531  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
532 
533  // Determine intersections between circles and limbos
534  double i11,i12,i21,i22,j11,j12,j21,j22;
535 
536  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
537  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
538 
539  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
540  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
541 
542  // Create the image points
543  vpImagePoint ip11,ip12,ip21,ip22;
544  ip11.set_ij(i11,j11);
545  ip12.set_ij(i12,j12);
546  ip21.set_ij(i21,j21);
547  ip22.set_ij(i22,j22);
548 
549  // Display
550  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
551  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
552 }
553 
563 void
564 vpMbtDistanceCylinder::display(const vpImage<vpRGBa>&I, const vpHomogeneousMatrix &cMo, const vpCameraParameters&cam, const vpColor col, const unsigned int thickness)
565 {
566  // Perspective projection
567  p1->changeFrame(cMo);
568  p2->changeFrame(cMo);
569  cercle1->changeFrame(cMo);
570  cercle2->changeFrame(cMo);
571  c->changeFrame(cMo);
572 
573  p1->projection();
574  p2->projection();
575  try{
576  cercle1->projection();
577  }
578  catch(...){std::cout<<"Problem projection circle 1";}
579  try{
580  cercle2->projection();
581  }
582  catch(...){std::cout<<"Problem projection circle 2";}
583  c->projection();
584 
585  double rho1,theta1;
586  double rho2,theta2;
587 
588  // Meters to pixels conversion
589  vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
590  vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
591 
592  // Determine intersections between circles and limbos
593  double i11,i12,i21,i22,j11,j12,j21,j22;
594 
595  getCylinderLineExtremity(i11, j11, rho1, theta1, cercle1);
596  getCylinderLineExtremity(i12, j12, rho1, theta1, cercle2);
597 
598  getCylinderLineExtremity(i21, j21, rho2, theta2, cercle1);
599  getCylinderLineExtremity(i22, j22, rho2, theta2, cercle2);
600 
601  // Create the image points
602  vpImagePoint ip11,ip12,ip21,ip22;
603  ip11.set_ij(i11,j11);
604  ip12.set_ij(i12,j12);
605  ip21.set_ij(i21,j21);
606  ip22.set_ij(i22,j22);
607 
608  // Display
609  vpDisplay::displayLine(I,ip11,ip12,col, thickness);
610  vpDisplay::displayLine(I,ip21,ip22,col, thickness);
611 }
612 
613 
624 void
625 vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
626 {
627  if (meline1 != NULL)
628  {
629  meline1->display(I);
630  }
631  if (meline2 != NULL)
632  {
633  meline2->display(I);
634  }
635 }
636 
640 void
641 vpMbtDistanceCylinder::initInteractionMatrixError()
642 {
643  nbFeaturel1 = (unsigned int)meline1->getMeList().size();
644  nbFeaturel2 = (unsigned int)meline2->getMeList().size();
645  nbFeature = nbFeaturel1 + nbFeaturel2;
646  L.resize(nbFeature, 6);
647  error.resize(nbFeature);
648 }
649 
653 void
654 vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
655 {
656  // Perspective projection
657  c->changeFrame(cMo) ;
658  c->projection() ;
659  cercle1->changeFrame(cMo) ;
660  cercle1->changeFrame(cMo) ;
661  try{
662  cercle1->projection();
663  }
664  catch(...){std::cout<<"Problem projection circle 1\n";}
665  try{
666  cercle2->projection();
667  }
668  catch(...){std::cout<<"Problem projection circle 2\n";}
669 
670  bool disp = false;
671  bool disp2 = false;
672  if (disp || disp2) vpDisplay::flush(I);
673 
674  // Build the lines
675  vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
676  vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
677 
678  double rho1 = featureline1.getRho() ;
679  double theta1 = featureline1.getTheta() ;
680  double rho2 = featureline2.getRho() ;
681  double theta2 = featureline2.getTheta() ;
682 
683  double co1 = cos(theta1);
684  double si1 = sin(theta1);
685  double co2 = cos(theta2);
686  double si2 = sin(theta2);
687 
688  double mx = 1.0/cam.get_px() ;
689  double my = 1.0/cam.get_py() ;
690  double xc = cam.get_u0() ;
691  double yc = cam.get_v0() ;
692 
693  double alpha1 ;
694  vpMatrix H1 ;
695  H1 = featureline1.interaction() ;
696  double alpha2 ;
697  vpMatrix H2 ;
698  H2 = featureline2.interaction() ;
699 
700  double x,y ;
701  vpMeSite p ;
702  unsigned int j =0 ;
703  for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
704  x = (double)it->j;
705  y = (double)it->i;
706 
707  x = (x-xc)*mx ;
708  y = (y-yc)*my ;
709 
710  alpha1 = x*si1 - y*co1;
711 
712  double *Lrho = H1[0] ;
713  double *Ltheta = H1[1] ;
714  // Calculate interaction matrix for a distance
715  for (unsigned int k=0 ; k < 6 ; k++){
716  L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
717  }
718  error[j] = rho1 - ( x*co1 + y*si1) ;
719 
720  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
721 
722  j++;
723  }
724 
725  for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
726  x = (double)it->j;
727  y = (double)it->i;
728 
729  x = (x-xc)*mx ;
730  y = (y-yc)*my ;
731 
732  alpha2 = x*si2 - y*co2;
733 
734  double *Lrho = H2[0] ;
735  double *Ltheta = H2[1] ;
736  // Calculate interaction matrix for a distance
737  for (unsigned int k=0 ; k < 6 ; k++){
738  L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
739  }
740  error[j] = rho2 - ( x*co2 + y*si2) ;
741 
742  if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
743 
744  j++;
745  }
746 }
747 
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpMbtDistanceCylinder::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:625
vpMbtDistanceCylinder::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:378
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:289
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:343
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:125
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:288
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:1991
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:167
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:177
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:290
vpMbtDistanceCylinder::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:479
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:202
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:285
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:505
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:654
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