ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpMbtDistanceLine.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbtDistanceLine.cpp 4311 2013-07-16 15:02:57Z ayol $
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  *
34  * Description:
35  * Make the complete tracking of an object by using its CAD model
36  *
37  * Authors:
38  * Nicolas Melchior
39  * Romain Tallonneau
40  * Eric Marchand
41  *
42  *****************************************************************************/
43 #include <visp/vpConfig.h>
44 
50 #include <visp/vpMbtDistanceLine.h>
51 #include <visp/vpPlane.h>
52 #include <visp/vpMeterPixelConversion.h>
53 #include <visp/vpFeatureBuilder.h>
54 #include <stdlib.h>
55 
60 {
61  name = "";
62  p1 = NULL ;
63  p2 = NULL ;
64  line = NULL ;
65  meline = NULL ;
66  hiddenface = NULL ;
67  wmean = 1 ;
68  nbFeature =0 ;
69  Reinit = false;
70  isvisible = false;
71 }
72 
77 {
78 // cout << "Deleting line " << index << endl ;
79  if (line != NULL) delete line ;
80  if (meline != NULL) delete meline ;
81 
82 }
83 
89 void
90 vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
91 {
92  line->project(cMo) ;
93  p1->project(cMo) ;
94  p2->project(cMo) ;
95 }
96 
105 void
106 buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
107 {
108  vpColVector a(3);
109  vpColVector b(3);
110  vpColVector n(3);
111  //Calculate vector corresponding to PQ
112  a[0]=P.get_oX()-Q.get_oX();
113  a[1]=P.get_oY()-Q.get_oY();
114  a[2]=P.get_oZ()-Q.get_oZ();
115 
116  //Calculate vector corresponding to PR
117  b[0]=P.get_oX()-R.get_oX();
118  b[1]=P.get_oY()-R.get_oY();
119  b[2]=P.get_oZ()-R.get_oZ();
120 
121  //Calculate normal vector to plane PQ x PR
122  n=vpColVector::cross(a,b);
123 
124  //Equation of the plane is given by:
125  double A = n[0];
126  double B = n[1];
127  double C = n[2];
128  double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
129 
130  double norm = sqrt(A*A+B*B+C*C) ;
131  plane.setA(A/norm) ;
132  plane.setB(B/norm) ;
133  plane.setC(C/norm) ;
134  plane.setD(D/norm) ;
135 }
136 
137 
149 void
150 buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
151 {
152  vpPlane plane1;
153  vpPlane plane2 ;
154  buildPlane(P1,P2,P3,plane1) ;
155  buildPlane(P1,P2,P4,plane2) ;
156 
157  L.setWorldCoordinates(plane1.getA(),plane1.getB(), plane1.getC(),plane1.getD(),
158  plane2.getA(),plane2.getB(), plane2.getC(),plane2.getD()) ;
159 }
160 
161 
168 void
170 {
171  line = new vpLine ;
172  poly.setNbPoint(2);
173  poly.addPoint(0, _p1);
174  poly.addPoint(1, _p2);
175 
176  p1 = &poly.p[0];
177  p2 = &poly.p[1];
178 
179  vpColVector V1(3);
180  vpColVector V2(3);
181  vpColVector V3(3);
182  vpColVector V4(3);
183 
184  V1[0] = p1->get_oX();
185  V1[1] = p1->get_oY();
186  V1[2] = p1->get_oZ();
187  V2[0] = p2->get_oX();
188  V2[1] = p2->get_oY();
189  V2[2] = p2->get_oZ();
190 
191  //if((V1-V2).sumSquare()!=0)
192  if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
193  {
194  {
195  V3[0]=double(rand()%1000)/100;
196  V3[1]=double(rand()%1000)/100;
197  V3[2]=double(rand()%1000)/100;
198 
199 
200  vpColVector v_tmp1,v_tmp2;
201  v_tmp1 = V2-V1;
202  v_tmp2 = V3-V1;
203  V4=vpColVector::cross(v_tmp1,v_tmp2);
204  }
205 
206  vpPoint P3;
207  P3.setWorldCoordinates(V3[0],V3[1],V3[2]);
208  vpPoint P4;
209  P4.setWorldCoordinates(V4[0],V4[1],V4[2]);
210  buildLine(*p1,*p2, P3,P4, *line) ;
211  }
212  else
213  {
214  vpPoint P3;
215  P3.setWorldCoordinates(V1[0],V1[1],V1[2]);
216  vpPoint P4;
217  P4.setWorldCoordinates(V2[0],V2[1],V2[2]);
218  buildLine(*p1,*p2,P3,P4,*line) ;
219  }
220 }
221 
222 
228 void
230 {
231  me = _me ;
232  if (meline != NULL)
233  {
234  meline->setMe(me) ;
235  }
236 }
237 
238 
246 void
248 {
249  if(isvisible){
250  p1->changeFrame(cMo);
251  p2->changeFrame(cMo);
252 
253  if(poly.getClipping() > 3) // Contains at least one FOV constraint
254  cam.computeFov(I.getWidth(), I.getHeight());
255 
256  poly.computeRoiClipped(cam);
257 
258  if(poly.roiPointsClip.size() == 2){ //Les points sont visibles.
259  vpImagePoint ip1, ip2;
260  double rho,theta;
261  line->changeFrame(cMo);
262  line->projection();
263 
264  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
265  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
266 
267  //rho theta uv
269 
270  while (theta > M_PI) { theta -= M_PI ; }
271  while (theta < -M_PI) { theta += M_PI ; }
272 
273  if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
274  else theta = M_PI/2.0 - theta;
275 
276  meline = new vpMbtMeLine ;
277  meline->setMe(me) ;
278 
279  // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
280  meline->setInitRange(0);
281 
282  int marge = /*10*/5; //ou 5 normalement
283  if (ip1.get_j()<ip2.get_j()) { meline->jmin = (int)ip1.get_j()-marge ; meline->jmax = (int)ip2.get_j()+marge ; } else{ meline->jmin = (int)ip2.get_j()-marge ; meline->jmax = (int)ip1.get_j()+marge ; }
284  if (ip1.get_i()<ip2.get_i()) { meline->imin = (int)ip1.get_i()-marge ; meline->imax = (int)ip2.get_i()+marge ; } else{ meline->imin = (int)ip2.get_i()-marge ; meline->imax = (int)ip1.get_i()+marge ; }
285 
286  try
287  {
288  meline->initTracking(I,ip1,ip2,rho,theta);
289  }
290  catch(...)
291  {
292  //vpTRACE("the line can't be initialized");
293  }
294  }
295  else{
296  if (meline!=NULL) delete meline;
297  meline=NULL;
298  isvisible = false;
299  }
300  }
301 // trackMovingEdge(I,cMo) ;
302 }
303 
304 
305 
312 void
314 {
315 
316  if (isvisible)
317  {
318 // p1->changeFrame(cMo) ;
319 // p2->changeFrame(cMo) ;
320 //
321 // p1->projection() ;
322 // p2->projection() ;
323 //
324 // vpImagePoint ip1, ip2;
325 //
326 // vpMeterPixelConversion::convertPoint(*cam,p1->get_x(),p1->get_y(),ip1) ;
327 // vpMeterPixelConversion::convertPoint(*cam,p2->get_x(),p2->get_y(),ip2) ;
328 //
329 // int marge = /*10*/5; //ou 5 normalement
330 // if (ip1.get_j()<ip2.get_j()) { meline->jmin = ip1.get_j()-marge ; meline->jmax = ip2.get_j()+marge ; }
331 // else{ meline->jmin = ip2.get_j()-marge ; meline->jmax = ip1.get_j()+marge ; }
332 // if (ip1.get_i()<ip2.get_i()) { meline->imin = ip1.get_i()-marge ; meline->imax = ip2.get_i()+marge ; }
333 // else{ meline->imin = ip2.get_i()-marge ; meline->imax = ip1.get_i()+marge ; }
334 
335  try
336  {
337  meline->track(I) ;
338  }
339  catch(...)
340  {
341  Reinit = true;
342  }
343  nbFeature =(unsigned int) meline->getMeList().size();
344  }
345 }
346 
347 
354 void
356 {
357  if(isvisible){
358  p1->changeFrame(cMo);
359  p2->changeFrame(cMo);
360 
361  if(poly.getClipping() > 3) // Contains at least one FOV constraint
362  cam.computeFov(I.getWidth(), I.getHeight());
363 
364  poly.computeRoiClipped(cam);
365 
366  if(poly.roiPointsClip.size() == 2){ //Les points sont visibles.
367  vpImagePoint ip1, ip2;
368  double rho,theta;
369  line->changeFrame(cMo);
370  line->projection();
371 
372  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
373  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
374 
375  //rho theta uv
377 
378  while (theta > M_PI) { theta -= M_PI ; }
379  while (theta < -M_PI) { theta += M_PI ; }
380 
381  if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
382  else theta = M_PI/2.0 - theta;
383 
384  int marge = /*10*/5; //ou 5 normalement
385  if (ip1.get_j()<ip2.get_j()) { meline->jmin = (int)ip1.get_j()-marge ; meline->jmax = (int)ip2.get_j()+marge ; } else{ meline->jmin = (int)ip2.get_j()-marge ; meline->jmax = (int)ip1.get_j()+marge ; }
386  if (ip1.get_i()<ip2.get_i()) { meline->imin = (int)ip1.get_i()-marge ; meline->imax = (int)ip2.get_i()+marge ; } else{ meline->imin = (int)ip2.get_i()-marge ; meline->imax = (int)ip1.get_i()+marge ; }
387 
388  try
389  {
390  //meline->updateParameters(I,rho,theta) ;
391  meline->updateParameters(I,ip1,ip2,rho,theta) ;
392  }
393  catch(...)
394  {
395  Reinit = true;
396  }
397  nbFeature = (unsigned int)meline->getMeList().size();
398  }
399  else{
400  if (meline!=NULL) delete meline;
401  meline=NULL;
402  isvisible = false;
403  }
404  }
405 }
406 
407 
416 void
418 {
419  if(meline!= NULL)
420  delete meline;
421 
422  initMovingEdge(I,cMo);
423 
424  Reinit = false;
425 }
426 
427 
438 void
439 vpMbtDistanceLine::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo, const vpCameraParameters&cam, const vpColor col, const unsigned int thickness, const bool displayFullModel)
440 {
441  p1->changeFrame(cMo);
442  p2->changeFrame(cMo);
443 
444  if(isvisible || displayFullModel){
445  vpImagePoint ip1, ip2;
446  vpCameraParameters c = cam;
447  if(poly.getClipping() > 3) // Contains at least one FOV constraint
448  c.computeFov(I.getWidth(), I.getHeight());
449 
450  poly.computeRoiClipped(c);
451 
452  if( poly.roiPointsClip.size() == 2 &&
453  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
454  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
455  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
456  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
457  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
458  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
459  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
460  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
461 
462  vpDisplay::displayLine(I,ip1,ip2,col, thickness);
463  }
464  }
465 }
466 
467 
478 void
479 vpMbtDistanceLine::display(const vpImage<vpRGBa>&I, const vpHomogeneousMatrix &cMo, const vpCameraParameters&cam, const vpColor col, const unsigned int thickness, const bool displayFullModel)
480 {
481  p1->changeFrame(cMo);
482  p2->changeFrame(cMo);
483 
484  if(isvisible || displayFullModel){
485  vpImagePoint ip1, ip2;
486  vpCameraParameters c = cam;
487  if(poly.getClipping() > 3) // Contains at least one FOV constraint
488  c.computeFov(I.getWidth(), I.getHeight());
489 
490  poly.computeRoiClipped(c);
491 
492  if( poly.roiPointsClip.size() == 2 &&
493  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::NEAR_CLIPPING) == 0) &&
494  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::FAR_CLIPPING) == 0) &&
495  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::DOWN_CLIPPING) == 0) &&
496  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::UP_CLIPPING) == 0) &&
497  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::LEFT_CLIPPING) == 0) &&
498  ((poly.roiPointsClip[1].second & poly.roiPointsClip[0].second & vpMbtPolygon::RIGHT_CLIPPING) == 0)){
499  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[0].first.get_x(),poly.roiPointsClip[0].first.get_y(),ip1);
500  vpMeterPixelConversion::convertPoint(cam,poly.roiPointsClip[1].first.get_x(),poly.roiPointsClip[1].first.get_y(),ip2);
501 
502  vpDisplay::displayLine(I,ip1,ip2,col, thickness);
503  }
504  }
505 }
506 
507 
518 void
520 {
521  if (meline != NULL)
522  {
523  meline->display(I);
524  }
525 }
526 
530 void
532 {
533  if (isvisible == true)
534  {
535  L.resize((unsigned int)meline->getMeList().size(),6) ;
536  error.resize((unsigned int)meline->getMeList().size()) ;
537  nbFeature = (unsigned int)meline->getMeList().size() ;
538  }
539  else
540  nbFeature = 0 ;
541 }
542 
546 void
548 {
549 
550  if (isvisible)
551  {
552  // feature projection
553  line->changeFrame(cMo) ;
554  line->projection() ;
555 
556  vpFeatureBuilder::create(featureline,*line) ;
557 
558  double rho = featureline.getRho() ;
559  double theta = featureline.getTheta() ;
560 
561  double co = cos(theta);
562  double si = sin(theta);
563 
564  double mx = 1.0/cam.get_px() ;
565  double my = 1.0/cam.get_py() ;
566  double xc = cam.get_u0() ;
567  double yc = cam.get_v0() ;
568 
569  double alpha ;
570  vpMatrix H ;
571  H = featureline.interaction() ;
572 
573  double x,y ;
574  vpMeSite p ;
575  unsigned int j =0 ;
576  for(std::list<vpMeSite>::const_iterator it=meline->getMeList().begin(); it!=meline->getMeList().end(); ++it){
577  x = (double)it->j ;
578  y = (double)it->i ;
579 
580  x = (x-xc)*mx ;
581  y = (y-yc)*my ;
582 
583  alpha = x*si - y*co;
584 
585  double *Lrho = H[0] ;
586  double *Ltheta = H[1] ;
587  // Calculate interaction matrix for a distance
588  for (unsigned int k=0 ; k < 6 ; k++)
589  {
590  L[j][k] = (Lrho[k] + alpha*Ltheta[k]);
591  }
592  error[j] = rho - ( x*co + y*si) ;
593  j++;
594  }
595  }
596 }
597 
605 bool
606 vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold)
607 {
608  if(threshold > I.getWidth() || threshold > I.getHeight()){
609  return true;
610  }
611  if (isvisible){
612 
613  for(std::list<vpMeSite>::const_iterator it=meline->getMeList().begin(); it!=meline->getMeList().end(); ++it){
614  int i = it->i ;
615  int j = it->j ;
616 
617  if(i < 0 || j < 0){ //out of image.
618  return true;
619  }
620 
621  if( ((unsigned int)i > (I.getHeight()- threshold) ) || (unsigned int)i < threshold ||
622  ((unsigned int)j > (I.getWidth ()- threshold) ) || (unsigned int)j < threshold ) {
623  return true;
624  }
625  }
626  }
627  return false;
628 }