ViSP  2.8.0
vpMbTracker.cpp
1 /****************************************************************************
2  *
3  * $Id: vpMbTracker.cpp 4323 2013-07-18 09:24:01Z 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  * Contact visp@irisa.fr if any conditions of this licensing are
34  * not clear to you.
35  *
36  * Description:
37  * Generic model based tracker
38  *
39  * Authors:
40  * Romain Tallonneau
41  * Aurelien Yol
42  * Eric Marchand
43  *
44  *****************************************************************************/
45 
52 #include <visp/vpMatrix.h>
53 #include <visp/vpMath.h>
54 #include <visp/vpColVector.h>
55 #include <visp/vpPoint.h>
56 #include <visp/vpPose.h>
57 #include <visp/vpDisplay.h>
58 #include <visp/vpDisplayOpenCV.h>
59 #include <visp/vpDisplayX.h>
60 #include <visp/vpDisplayGDI.h>
61 #include <visp/vpPixelMeterConversion.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpColor.h>
64 #include <visp/vpIoTools.h>
65 #include <visp/vpException.h>
66 #include <visp/vpImageIo.h>
67 #include <visp/vpMbTracker.h>
68 #include <visp/vpMatrixException.h>
69 #include <visp/vpIoTools.h>
70 #include <iostream>
71 #include <limits>
72 #ifdef VISP_HAVE_COIN
73 //Inventor includes
74 #include <Inventor/nodes/SoSeparator.h>
75 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
76 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
77 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
78 #include <Inventor/actions/SoWriteAction.h>
79 #include <Inventor/actions/SoSearchAction.h>
80 #include <Inventor/misc/SoChildList.h>
81 #include <Inventor/actions/SoGetMatrixAction.h>
82 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
83 #include <Inventor/actions/SoToVRML2Action.h>
84 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
85 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
86 #include <Inventor/VRMLnodes/SoVRMLShape.h>
87 #endif
88 
89 
96 {
97  modelInitialised = false;
98  computeCovariance = false;
99  displayFeatures = false;
100 }
101 
106 {
107 }
108 
109 
131 void
132 vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp)
133 {
134  vpHomogeneousMatrix last_cMo;
135  vpPoseVector init_pos;
136  vpImagePoint ip;
138 
139  std::string ext = ".init";
140  std::string str_pose = "";
141  size_t pos = (unsigned int)initFile.rfind(ext);
142 
143  // Load the last poses from files
144  std::fstream finitpos ;
145  std::fstream finit ;
146  char s[FILENAME_MAX];
147  if(poseSavingFilename.empty()){
148  if( pos == initFile.size()-ext.size() && pos != 0)
149  str_pose = initFile.substr(0,pos) + ".0.pos";
150  else
151  str_pose = initFile + ".0.pos";
152 
153  finitpos.open(str_pose.c_str() ,std::ios::in) ;
154  sprintf(s, "%s", str_pose.c_str());
155  }else{
156  finitpos.open(poseSavingFilename.c_str() ,std::ios::in) ;
157  sprintf(s, "%s", poseSavingFilename.c_str());
158  }
159  if(finitpos.fail() ){
160  std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
161  last_cMo.setIdentity();
162  }
163  else{
164  for (unsigned int i = 0; i < 6; i += 1){
165  finitpos >> init_pos[i];
166  }
167 
168  finitpos.close();
169  last_cMo.buildFrom(init_pos) ;
170 
171  std::cout <<"last_cMo : "<<std::endl << last_cMo <<std::endl;
172 
174  display(I, last_cMo, cam, vpColor::green, 1, true);
175  vpDisplay::displayFrame(I, last_cMo, cam, 0.05, vpColor::green);
176  vpDisplay::flush(I);
177 
178  std::cout << "No modification : left click " << std::endl;
179  std::cout << "Modify initial pose : right click " << std::endl ;
180 
182  "left click to validate, right click to modify initial pose",
183  vpColor::red);
184 
185  vpDisplay::flush(I) ;
186 
187  while (!vpDisplay::getClick(I, ip, button)) ;
188  }
189 
190 
191  if (!finitpos.fail() && button == vpMouseButton::button1){
192  cMo = last_cMo ;
193  }
194  else
195  {
196  vpDisplay::display(I) ;
197  vpDisplay::flush(I) ;
198 
199  vpPose pose ;
200 
201  pose.clearPoint() ;
202 
203  // file parser
204  // number of points
205  // X Y Z
206  // X Y Z
207 
208  double X,Y,Z ;
209 
210  if( pos == initFile.size()-ext.size() && pos != 0)
211  sprintf(s,"%s", initFile.c_str());
212  else
213  sprintf(s,"%s.init", initFile.c_str());
214 
215  std::cout << "filename " << s << std::endl ;
216  finit.open(s,std::ios::in) ;
217  if (finit.fail()){
218  std::cout << "cannot read " << s << std::endl;
219  throw vpException(vpException::ioError, "cannot read init file");
220  }
221 
222  std::string dispF;
223  if( pos == initFile.size()-ext.size() && pos != 0)
224  dispF = initFile.substr(0,pos) + ".ppm";
225  else
226  dispF = initFile + ".ppm";
227 
228  sprintf(s, "%s", dispF.c_str());
229 
230  vpImage<vpRGBa> Iref ;
231  //Display window creation and initialistation
232 #if defined VISP_HAVE_X11
233  vpDisplayX d;
234 #elif defined VISP_HAVE_GDI
235  vpDisplayGDI d;
236 #elif defined VISP_HAVE_OPENCV
237  vpDisplayOpenCV d;
238 #endif
239  try{
240  if(displayHelp){
241  vpImageIo::read(Iref,s) ;
242 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
243  d.init(Iref,10,500, "Where to initialize...") ;
244  vpDisplay::display(Iref) ;
245  vpDisplay::flush(Iref);
246 #endif
247  }
248  }
249  catch(...){}
250 
251  unsigned int n ;
252  finit >> n ;
253  std::cout << "number of points " << n << std::endl ;
254  vpPoint *P = new vpPoint [n] ;
255  for (unsigned int i=0 ; i < n ; i++){
256  finit >> X ;
257  finit >> Y ;
258  finit >> Z ;
259  P[i].setWorldCoordinates(X,Y,Z) ; // (X,Y,Z)
260  }
261 
262  finit.close();
263 
265  bool isWellInit = false;
266  while(!isWellInit)
267  {
269  for(unsigned int i=0 ; i< n ; i++)
270  {
271  std::cout << "Click on point " << i+1 << std::endl ;
272  double x=0,y=0;
273  vpDisplay::getClick(I, ip) ;
275  vpDisplay::flush(I) ;
277  P[i].set_x(x);
278  P[i].set_y(y);
279 
280  std::cout << "click sur point " << ip << std::endl;
281 
282  vpDisplay::displayPoint (I, ip, vpColor::green); //display target point
283  pose.addPoint(P[i]) ; // and added to the pose computation point list
284  }
285  vpDisplay::flush(I) ;
286 
287  vpHomogeneousMatrix cMo1, cMo2;
288  pose.computePose(vpPose::LAGRANGE, cMo1) ;
289  double d1 = pose.computeResidual(cMo1);
290  pose.computePose(vpPose::DEMENTHON, cMo2) ;
291  double d2 = pose.computeResidual(cMo2);
292 
293  if(d1 < d2){
294  cMo = cMo1;
295  }
296  else{
297  cMo = cMo2;
298  }
300 
301  display(I, cMo, cam, vpColor::green, 1, true);
303  "left click to validate, right click to re initialize object",
304  vpColor::red);
305 
306  vpDisplay::flush(I) ;
307 
309  while (!vpDisplay::getClick(I, ip, button)) ;
310 
311 
312  if (button == vpMouseButton::button1)
313  {
314  isWellInit = true;
315  }
316  else
317  {
318  pose.clearPoint() ;
319  vpDisplay::display(I) ;
320  vpDisplay::flush(I) ;
321  }
322  }
325 
326  delete [] P;
327 
328  //save the pose into file
329  if(poseSavingFilename.empty())
330  savePose(str_pose);
331  else
333  }
334 
335  std::cout <<"cMo : "<<std::endl << cMo <<std::endl;
336 
337  init(I);
338 }
339 
348 void vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
349  const std::string &displayFile)
350 {
351  vpDisplay::display(I) ;
352  vpDisplay::flush(I) ;
353 
354  vpPose pose ;
355  vpPoint *P = NULL; P = new vpPoint [points3D_list.size()] ;
356  for (unsigned int i=0 ; i < points3D_list.size() ; i++)
357  P[i].setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
358 
359  vpImage<vpRGBa> Iref ;
360  //Display window creation and initialistation
361  #if defined VISP_HAVE_X11
362  vpDisplayX d;
363  #elif defined VISP_HAVE_GDI
364  vpDisplayGDI d;
365  #elif defined VISP_HAVE_OPENCV
366  vpDisplayOpenCV d;
367  #endif
368 
369  if(displayFile != ""){
370  try{
371  std::cout << displayFile.c_str() << std::endl;
372  vpImageIo::read(Iref,displayFile.c_str()) ;
373  #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
374  d.init(Iref,10,500, "Where to initialize...") ;
375  vpDisplay::display(Iref) ;
376  vpDisplay::flush(Iref);
377  #endif
378  }
379  catch(...){}
380  }
381 
382  vpImagePoint ip;
383  bool isWellInit = false;
384  while(!isWellInit)
385  {
386  for(unsigned int i=0 ; i< points3D_list.size() ; i++)
387  {
388  std::cout << "Click on point " << i+1 << std::endl ;
389  double x=0,y=0;
390  vpDisplay::getClick(I, ip) ;
392  vpDisplay::flush(I) ;
394  P[i].set_x(x);
395  P[i].set_y(y);
396 
397  std::cout << "Click on point " << ip << std::endl;
398 
399  vpDisplay::displayPoint (I, ip, vpColor::green); //display target point
400  pose.addPoint(P[i]) ; // and added to the pose computation point list
401  }
402  vpDisplay::flush(I) ;
403 
404  vpHomogeneousMatrix cMo1, cMo2;
405  pose.computePose(vpPose::LAGRANGE, cMo1) ;
406  double d1 = pose.computeResidual(cMo1);
407  pose.computePose(vpPose::DEMENTHON, cMo2) ;
408  double d2 = pose.computeResidual(cMo2);
409 
410  if(d1 < d2){
411  cMo = cMo1;
412  }
413  else{
414  cMo = cMo2;
415  }
417 
418  display(I, cMo, cam, vpColor::green, 1, true);
420  "left click to validate, right click to re initialize object",
421  vpColor::red);
422 
423  vpDisplay::flush(I) ;
424 
426  while (!vpDisplay::getClick(I, ip, button)) ;
427 
428 
429  if (button == vpMouseButton::button1)
430  {
431  isWellInit = true;
432  }
433  else
434  {
435  pose.clearPoint() ;
436  vpDisplay::display(I) ;
437  vpDisplay::flush(I) ;
438  }
439  }
440 
442 
443  delete [] P;
444 
445  init(I);
446 }
447 
465 void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile )
466 {
467  char s[FILENAME_MAX];
468  std::fstream finit ;
469 
470  std::string ext = ".init";
471  size_t pos = initFile.rfind(ext);
472 
473  if( pos == initFile.size()-ext.size() && pos != 0)
474  sprintf(s,"%s", initFile.c_str());
475  else
476  sprintf(s,"%s.init", initFile.c_str());
477 
478  std::cout << "filename " << s << std::endl ;
479  finit.open(s,std::ios::in) ;
480  if (finit.fail()){
481  std::cout << "cannot read " << s << std::endl;
482  throw vpException(vpException::ioError, "cannot read init file");
483  }
484 
485  unsigned int size;
486  double X, Y, Z;
487  finit >> size ;
488  std::cout << "number of points " << size << std::endl ;
489  vpPoint *P = new vpPoint [size];
490  vpPose pose ;
491 
492  for(unsigned int i=0 ; i< size ; i++)
493  {
494  finit >> X ;
495  finit >> Y ;
496  finit >> Z ;
497  P[i].setWorldCoordinates(X,Y,Z) ;
498  }
499 
500  unsigned int size2;
501  double x, y;
502  vpImagePoint ip;
503  finit >> size2 ;
504  if(size != size2)
505  vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
506 
507  for(unsigned int i=0 ; i< size ; i++)
508  {
509  finit >> x;
510  finit >> y;
511  ip = vpImagePoint(x,y);
513  P[i].set_x(x);
514  P[i].set_y(y);
515  pose.addPoint(P[i]);
516  }
517 
518  vpHomogeneousMatrix cMo1, cMo2;
519  pose.computePose(vpPose::LAGRANGE, cMo1) ;
520  double d1 = pose.computeResidual(cMo1);
521  pose.computePose(vpPose::DEMENTHON, cMo2) ;
522  double d2 = pose.computeResidual(cMo2);
523 
524  if(d1 < d2)
525  cMo = cMo1;
526  else
527  cMo = cMo2;
528 
530 
531  delete [] P;
532 
533  init(I);
534 }
535 
544 void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list,
545  const std::vector<vpPoint> &points3D_list )
546 {
547  if(points2D_list.size() != points3D_list.size())
548  vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
549 
550  size_t size = points3D_list.size();
551  vpPoint *P = new vpPoint [size];
552  vpPose pose ;
553 
554  for(size_t i=0 ; i< size ; i++)
555  {
556  P[i].setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
557  double x=0,y=0;
558  vpPixelMeterConversion::convertPoint(cam, points2D_list[i], x, y);
559  P[i].set_x(x);
560  P[i].set_y(y);
561  pose.addPoint(P[i]);
562  }
563 
564  vpHomogeneousMatrix cMo1, cMo2;
565  pose.computePose(vpPose::LAGRANGE, cMo1) ;
566  double d1 = pose.computeResidual(cMo1);
567  pose.computePose(vpPose::DEMENTHON, cMo2) ;
568  double d2 = pose.computeResidual(cMo2);
569 
570  if(d1 < d2)
571  cMo = cMo1;
572  else
573  cMo = cMo2;
574 
576 
577  delete [] P;
578 
579  init(I);
580 }
581 
599 void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const std::string &initFile)
600 {
601  char s[FILENAME_MAX];
602  std::fstream finit ;
603  vpPoseVector init_pos;
604 
605  std::string ext = ".pos";
606  size_t pos = initFile.rfind(ext);
607 
608  if( pos == initFile.size()-ext.size() && pos != 0)
609  sprintf(s,"%s", initFile.c_str());
610  else
611  sprintf(s,"%s.pos", initFile.c_str());
612 
613  std::cout << "filename " << s << std::endl ;
614  finit.open(s,std::ios::in) ;
615  if (finit.fail()){
616  std::cout << "cannot read " << s << std::endl;
617  throw vpException(vpException::ioError, "cannot read init file");
618  }
619 
620  for (unsigned int i = 0; i < 6; i += 1){
621  finit >> init_pos[i];
622  }
623 
624  cMo.buildFrom(init_pos);
625  init(I);
626 }
627 
635 {
636  this->cMo = cMo;
637  init(I);
638 }
639 
647 {
648  vpHomogeneousMatrix _cMo(cPo);
649  initFromPose(I, _cMo);
650 }
651 
657 void vpMbTracker::savePose(const std::string &filename)
658 {
659  vpPoseVector init_pos;
660  std::fstream finitpos ;
661  char s[FILENAME_MAX];
662 
663  sprintf(s,"%s", filename.c_str());
664  finitpos.open(s, std::ios::out) ;
665 
666  init_pos.buildFrom(cMo);
667  finitpos << init_pos;
668  finitpos.close();
669 }
670 
694 void
695 vpMbTracker::loadModel(const std::string& modelFile)
696 {
697  std::string::const_iterator it;
698 
699  if(vpIoTools::checkFilename(modelFile)){
700  it = modelFile.end();
701  if((*(it-1) == 'o' && *(it-2) == 'a' && *(it-3) == 'c' && *(it-4) == '.') ||
702  (*(it-1) == 'O' && *(it-2) == 'A' && *(it-3) == 'C' && *(it-4) == '.') ){
703  loadCAOModel(modelFile);
704  }
705  else if((*(it-1) == 'l' && *(it-2) == 'r' && *(it-3) == 'w' && *(it-4) == '.') ||
706  (*(it-1) == 'L' && *(it-2) == 'R' && *(it-3) == 'W' && *(it-4) == '.') ){
707  loadVRMLModel(modelFile);
708  }
709  else{
710  throw vpException(vpException::ioError, "file cannot be open");
711  }
712  }
713  else{
714  throw vpException(vpException::ioError, "file cannot be open");
715  }
716 
717  this->modelInitialised = true;
718  this->modelFileName = modelFile;
719 }
720 
721 
752 void
753 vpMbTracker::loadVRMLModel(const std::string& modelFile)
754 {
755 #ifdef VISP_HAVE_COIN
756  SoDB::init(); // Call SoDD::finish() before ending the program.
757 
758  SoInput in;
759  SbBool ok = in.openFile(modelFile.c_str());
760  SoSeparator *sceneGraph;
761  SoVRMLGroup *sceneGraphVRML2;
762 
763  if (!ok) {
764  vpERROR_TRACE("can't open file to load model");
765  throw vpException(vpException::fatalError, "can't open file to load model");
766  }
767 
768  if(!in.isFileVRML2())
769  {
770  sceneGraph = SoDB::readAll(&in);
771  if (sceneGraph == NULL) { /*return -1;*/ }
772  sceneGraph->ref();
773 
774  SoToVRML2Action tovrml2;
775  tovrml2.apply(sceneGraph);
776 
777  sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
778  sceneGraphVRML2->ref();
779  sceneGraph->unref();
780  }
781  else
782  {
783  sceneGraphVRML2 = SoDB::readAllVRML(&in);
784  if (sceneGraphVRML2 == NULL) { /*return -1;*/ }
785  sceneGraphVRML2->ref();
786  }
787 
788  in.closeFile();
789 
790  vpHomogeneousMatrix transform;
791  unsigned int indexFace = 0;
792  extractGroup(sceneGraphVRML2, transform, indexFace);
793 
794  sceneGraphVRML2->unref();
795 #else
796  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
797  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
798 #endif
799 }
800 
827 void
828 vpMbTracker::loadCAOModel(const std::string& modelFile)
829 {
830  std::ifstream fileId;
831  fileId.exceptions ( std::ifstream::failbit | std::ifstream::eofbit );
832  fileId.open (modelFile.c_str(), std::ifstream::in);
833  if(fileId.fail()) {
834  std::cout << "cannot read CAO model file: " << modelFile << std::endl;
835  throw vpException(vpException::ioError, "cannot read CAO model file");
836  }
837 
838  try{
839  char c;
840  // Extraction of the version (remove empty line and commented ones (comment
841  // line begin with the #)).
842  while( (fileId.get(c)!=NULL)&&(c == '#')) fileId.ignore(256,'\n');
843  fileId.unget();
844 
845  int caoVersion;
846  fileId.get(c);
847  if(c=='V'){
848  fileId >> caoVersion;
849  }
850  else{
851  std::cout <<"in vpMbEdgeTracker::loadCAOModel -> Bad parameter header file : use V0, V1, ...";
853  "in vpMbEdgeTracker::loadCAOModel -> Bad parameter header file : use V0, V1, ...");
854  }
855 
856  while( (fileId.get(c)!=NULL)&&(c!='\n')) ;
857  while( (fileId.get(c)!=NULL)&&(c == '#')) fileId.ignore(256,'\n') ;
858  fileId.unget();
859 
860  //Read the points
861  unsigned int caoNbrPoint;
862  fileId >> caoNbrPoint;
863  std::cout << "> " << caoNbrPoint << " points" << std::endl;
864  vpPoint *caoPoints = NULL;
865  if (caoNbrPoint > 0)
866  caoPoints = new vpPoint[caoNbrPoint];
867 
868  double x ; // 3D coordinates
869  double y ;
870  double z ;
871 
872  int i ; // image coordinate (used for matching)
873  int j ;
874 
875 
876  for(unsigned int k=0; k < caoNbrPoint; k++){
877  fileId >> x ;
878  fileId >> y ;
879  fileId >> z ;
880  if (caoVersion == 2){
881  fileId >> i ;
882  fileId >> j ;
883  }
884 
885  if(k != caoNbrPoint-1){// the rest of the line is removed (not the last one due to the need to remove possible comments).
886  fileId.ignore(256,'\n');
887  }
888  caoPoints[k].setWorldCoordinates(x, y, z) ;
889  }
890 
891  while( (fileId.get(c)!=NULL)&&(c!='\n')) ;
892  while( (fileId.get(c)!=NULL)&&(c == '#')) fileId.ignore(256,'\n');
893  fileId.unget();
894 
895  //Read the lines
896  unsigned int caoNbrLine;
897  fileId >> caoNbrLine;
898  unsigned int *caoLinePoints = NULL;
899  std::cout << "> " << caoNbrLine << " lines" << std::endl;
900  if (caoNbrLine > 0)
901  caoLinePoints = new unsigned int[2*caoNbrLine];
902 
903  unsigned int index1, index2;
904 
905  for(unsigned int k=0; k < caoNbrLine ; k++){
906  fileId >> index1 ;
907  fileId >> index2 ;
908 
909  caoLinePoints[2*k] = index1;
910  caoLinePoints[2*k+1] = index2;
911 
912  if(index1 < caoNbrPoint && index2 < caoNbrPoint){
913  std::vector<vpPoint> extremities;
914  extremities.push_back(caoPoints[index1]);
915  extremities.push_back(caoPoints[index2]);
916  initFaceFromCorners(extremities, k);
917  }
918  else{
919  vpTRACE(" line %d has wrong coordinates.", k);
920  }
921 
922  if(k != caoNbrLine-1){// the rest of the line is removed (not the last one due to the need to remove possible comments).
923  fileId.ignore(256,'\n');
924  }
925  }
926 
927  while( (fileId.get(c)!=NULL)&&(c!='\n')) ;
928  while( (fileId.get(c)!=NULL)&&(c == '#')) fileId.ignore(256,'\n');
929  fileId.unget();
930 
931 
932  /* Load polygon from the lines extracted earlier
933  (the first point of the line is used)*/
934  unsigned int caoNbrPolygonLine;
935  fileId >> caoNbrPolygonLine;
936  std::cout << "> " << caoNbrPolygonLine << " polygon line" << std::endl;
937  unsigned int index;
938  for(unsigned int k = 0;k < caoNbrPolygonLine; k++){
939  unsigned int nbLinePol;
940  fileId >> nbLinePol;
941  std::vector<vpPoint> corners;
942  for(unsigned int i = 0; i < nbLinePol; i++){
943  fileId >> index;
944  corners.push_back(caoPoints[caoLinePoints[2*index]]);
945  }
946  if(k != caoNbrPolygonLine-1){// the rest of the line is removed (not the last one due to the need to remove possible comments).
947  fileId.ignore(256,'\n');
948  }
949  initFaceFromCorners(corners, k);
950  }
951 
952  while( (fileId.get(c)!=NULL)&&(c!='\n')) ;
953  while( (fileId.get(c)!=NULL)&&(c == '#')) fileId.ignore(256,'\n');
954  fileId.unget();
955 
956  /* Extract the polygon using the point coordinates (top of the file) */
957  unsigned int caoNbrPolygonPoint;
958  fileId >> caoNbrPolygonPoint;
959  std::cout << "> " << caoNbrPolygonPoint << " polygon point" << std::endl;
960  for(unsigned int k = 0;k < caoNbrPolygonPoint; k++){
961  int nbPointPol;
962  fileId >> nbPointPol;
963  std::vector<vpPoint> corners;
964  for(int i = 0; i < nbPointPol; i++){
965  fileId >> index;
966  corners.push_back(caoPoints[index]);
967  }
968  if(k != caoNbrPolygonPoint-1){// the rest of the line is removed (not the last one due to the need to remove possible comments).
969  fileId.ignore(256,'\n');
970  }
971  initFaceFromCorners(corners, k);
972  }
973 
974  unsigned int caoNbCylinder;
975  try{
976  while( (fileId.get(c)!=NULL)&&(c!='\n')) ;
977  while( (fileId.get(c)!=NULL)&&(c == '#')) fileId.ignore(256,'\n');
978  fileId.unget();
979 
980  if(fileId.eof()){// check if not at the end of the file (for old style files)
981  delete[] caoPoints;
982  delete[] caoLinePoints;
983  return ;
984  }
985 
986  /* Extract the cylinders */
987 
988 
989  fileId >> caoNbCylinder;
990  std::cout << "> " << caoNbCylinder << " cylinder" << std::endl;
991  for(unsigned int k=0; k<caoNbCylinder; ++k){
992  double radius;
993  unsigned int indexP1, indexP2;
994  fileId >> indexP1;
995  fileId >> indexP2;
996  fileId >> radius;
997  initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius);
998  }
999 
1000  }catch(...){
1001  std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
1002  caoNbCylinder = 0;
1003  }
1004 
1005  delete[] caoPoints;
1006  delete[] caoLinePoints;
1007  }catch(std::ifstream::failure e){
1008  std::cerr << "Cannot read line!" << std::endl;
1009  throw vpException(vpException::ioError, "cannot read line");
1010  }
1011 
1012 }
1013 
1014 
1015 
1016 
1017 #ifdef VISP_HAVE_COIN
1018 
1025 void
1026 vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, unsigned int &indexFace)
1027 {
1028  vpHomogeneousMatrix transformCur;
1029  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
1030  if(sceneGraphVRML2Trasnform){
1031  float rx, ry, rz, rw;
1032  sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx,ry,rz,rw);
1033  vpRotationMatrix rotMat(vpQuaternionVector(rx,ry,rz,rw));
1034 // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " << rw << std::endl;
1035 
1036  float tx, ty, tz;
1037  tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
1038  ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
1039  tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
1040  vpTranslationVector transVec(tx,ty,tz);
1041 // std::cout << "Translation: " << tx << " " << ty << " " << tz << std::endl;
1042 
1043  float sx, sy, sz;
1044  sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
1045  sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
1046  sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
1047 // std::cout << "Scale: " << sx << " " << sy << " " << sz << std::endl;
1048 
1049  for(unsigned int i = 0 ; i < 3 ; i++)
1050  rotMat[0][i] *= sx;
1051  for(unsigned int i = 0 ; i < 3 ; i++)
1052  rotMat[1][i] *= sy;
1053  for(unsigned int i = 0 ; i < 3 ; i++)
1054  rotMat[2][i] *= sz;
1055 
1056  transformCur = vpHomogeneousMatrix(transVec,rotMat);
1057  transform = transform * transformCur;
1058  }
1059 
1060  int nbShapes = sceneGraphVRML2->getNumChildren();
1061 // std::cout << sceneGraphVRML2->getTypeId().getName().getString() << std::endl;
1062 // std::cout << "Nb object in VRML : " << nbShapes << std::endl;
1063 
1064  SoNode * child;
1065 
1066  for (int i = 0; i < nbShapes; i++)
1067  {
1068  vpHomogeneousMatrix transform_recursive(transform);
1069  child = sceneGraphVRML2->getChild(i);
1070 
1071  if (child->getTypeId() == SoVRMLGroup::getClassTypeId()){
1072  extractGroup((SoVRMLGroup*)child, transform_recursive, indexFace);
1073  }
1074 
1075  if (child->getTypeId() == SoVRMLTransform::getClassTypeId()){
1076  extractGroup((SoVRMLTransform*)child, transform_recursive, indexFace);
1077  }
1078 
1079  if (child->getTypeId() == SoVRMLShape::getClassTypeId()){
1080  SoChildList * child2list = child->getChildren();
1081  for (int j = 0; j < child2list->getLength(); j++)
1082  {
1083  if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
1084  {
1085  SoVRMLIndexedFaceSet * face_set;
1086  face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
1087  if(!strncmp(face_set->getName().getString(),"cyl",3)){
1088  extractCylinders(face_set, transform);
1089  }else{
1090  extractFaces(face_set, transform, indexFace);
1091  }
1092  }
1093  if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
1094  {
1095  SoVRMLIndexedLineSet * line_set;
1096  line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
1097  extractLines(line_set);
1098  }
1099  }
1100  }
1101  }
1102 }
1103 
1112 void
1113 vpMbTracker::extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, unsigned int &indexFace)
1114 {
1115  std::vector<vpPoint> corners;
1116  corners.resize(0);
1117 
1118 // SoMFInt32 indexList = _face_set->coordIndex;
1119 // int indexListSize = indexList.getNum();
1120  int indexListSize = face_set->coordIndex.getNum();
1121 
1122  vpColVector pointTransformed(4);
1123  vpPoint pt;
1124  SoVRMLCoordinate *coord;
1125 
1126  for (int i = 0; i < indexListSize; i++)
1127  {
1128  if (face_set->coordIndex[i] == -1)
1129  {
1130  if(corners.size() > 1)
1131  {
1132  initFaceFromCorners(corners, indexFace);
1133  indexFace++;
1134  corners.resize(0);
1135  }
1136  }
1137  else
1138  {
1139  coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
1140  int index = face_set->coordIndex[i];
1141  pointTransformed[0]=coord->point[index].getValue()[0];
1142  pointTransformed[1]=coord->point[index].getValue()[1];
1143  pointTransformed[2]=coord->point[index].getValue()[2];
1144  pointTransformed[3] = 1.0;
1145 
1146  pointTransformed = transform * pointTransformed;
1147 
1148  pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
1149  corners.push_back(pt);
1150  }
1151  }
1152 }
1153 
1166 void
1167 vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform)
1168 {
1169  std::vector<vpPoint> corners_c1, corners_c2;//points belonging to the first circle and to the second one.
1170  SoVRMLCoordinate* coords = (SoVRMLCoordinate *)face_set->coord.getValue();
1171 
1172  unsigned int indexListSize = (unsigned int)coords->point.getNum();
1173 
1174  if(indexListSize % 2 == 1){
1175  std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
1176  throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
1177  }
1178  corners_c1.resize(indexListSize / 2);
1179  corners_c2.resize(indexListSize / 2);
1180  vpColVector pointTransformed(4);
1181  vpPoint pt;
1182 
1183 
1184  // extract all points and fill the two sets.
1185  for(int i=0; i<coords->point.getNum(); ++i){
1186  pointTransformed[0]=coords->point[i].getValue()[0];
1187  pointTransformed[1]=coords->point[i].getValue()[1];
1188  pointTransformed[2]=coords->point[i].getValue()[2];
1189  pointTransformed[3] = 1.0;
1190 
1191  pointTransformed = transform * pointTransformed;
1192 
1193  pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
1194 
1195  if(i < (int)corners_c1.size()){
1196  corners_c1[(unsigned int)i] = pt;
1197  }else{
1198  corners_c2[(unsigned int)i-corners_c1.size()] = pt;
1199  }
1200  }
1201 
1202  vpPoint p1 = getGravityCenter(corners_c1);
1203  vpPoint p2 = getGravityCenter(corners_c2);
1204 
1205  vpColVector dist(3);
1206  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
1207  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
1208  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
1209  double radius_c1 = sqrt(dist.sumSquare());
1210  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
1211  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
1212  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
1213  double radius_c2 = sqrt(dist.sumSquare());
1214 
1215  if(std::fabs(radius_c1 - radius_c2) > (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))){
1216  std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
1217  throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
1218  }
1219 
1220  initCylinder(p1, p2, radius_c1);
1221 
1222 }
1223 
1233 vpPoint
1234 vpMbTracker::getGravityCenter(const std::vector<vpPoint>& pts)
1235 {
1236  if(pts.empty()){
1237  std::cout << "Cannot extract center of gravity of empty set." << std::endl;
1238  throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
1239  }
1240  double oX = 0;
1241  double oY = 0;
1242  double oZ = 0;
1243  vpPoint G;
1244 
1245  for(unsigned int i=0; i<pts.size(); ++i){
1246  oX += pts[i].get_oX();
1247  oY += pts[i].get_oY();
1248  oZ += pts[i].get_oZ();
1249  }
1250 
1251  G.setWorldCoordinates(oX/pts.size(), oY/pts.size(), oZ/pts.size());
1252  return G;
1253 }
1254 
1255 
1262 void
1263 vpMbTracker::extractLines(SoVRMLIndexedLineSet* line_set)
1264 {
1265  std::vector<vpPoint> corners;
1266  corners.resize(0);
1267 
1268  int indexListSize = line_set->coordIndex.getNum();
1269 
1270  SbVec3f point(0,0,0);
1271  vpPoint pt;
1272  SoVRMLCoordinate *coord;
1273 
1274  unsigned int indexFace = 0;
1275 
1276  for (int i = 0; i < indexListSize; i++)
1277  {
1278  if (line_set->coordIndex[i] == -1)
1279  {
1280  if(corners.size() > 1)
1281  {
1282  initFaceFromCorners(corners, indexFace);
1283  indexFace++;
1284  corners.resize(0);
1285  }
1286  }
1287  else
1288  {
1289  coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
1290  int index = line_set->coordIndex[i];
1291  point[0]=coord->point[index].getValue()[0];
1292  point[1]=coord->point[index].getValue()[1];
1293  point[2]=coord->point[index].getValue()[2];
1294 
1295  pt.setWorldCoordinates(point[0],point[1],point[2]);
1296  corners.push_back(pt);
1297  }
1298  }
1299 }
1300 
1301 #endif
1302 
1317 void
1318 vpMbTracker::computeJTR(const vpMatrix& interaction, const vpColVector& error, vpMatrix& JTR)
1319 {
1320  if(interaction.getRows() != error.getRows() || interaction.getCols() != 6 ){
1322  "Incorrect matrices size in computeJTR.");
1323  }
1324 
1325  JTR.resize(6, 1);
1326  const unsigned int N = interaction.getRows();
1327 
1328  for (unsigned int i = 0; i < 6; i += 1){
1329  double ssum = 0;
1330  for (unsigned int j = 0; j < N; j += 1){
1331  ssum += interaction[j][i] * error[j];
1332  }
1333  JTR[i][0] = ssum;
1334  }
1335 }
1336 
1337 
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
virtual ~vpMbTracker()
virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform)
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
virtual void extractLines(SoVRMLIndexedLineSet *line_set)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
virtual void initFaceFromCorners(const std::vector< vpPoint > &corners, const unsigned int indexFace=-1)=0
#define vpTRACE
Definition: vpDebug.h:401
void setIdentity()
Basic initialisation (identity)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:133
vpPoint getGravityCenter(const std::vector< vpPoint > &_pts)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:108
Define the X11 console to display images.
Definition: vpDisplayX.h:152
bool modelInitialised
Flag used to ensure that the CAD model is loaded before the initialisation.
Definition: vpMbTracker.h:112
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
error that can be emited by ViSP classes.
Definition: vpException.h:75
void computeJTR(const vpMatrix &J, const vpColVector &R, vpMatrix &JTR)
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.h:194
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, unsigned int &indexFace)
std::string modelFileName
The name of the file containing the model (it is used to create a file name.0.pos used to store the c...
Definition: vpMbTracker.h:110
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:760
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:116
static const vpColor green
Definition: vpColor.h:170
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1991
static const vpColor red
Definition: vpColor.h:167
Class that defines what is a point.
Definition: vpPoint.h:65
virtual void loadCAOModel(const std::string &modelFile)
vpPoseVector buildFrom(const vpHomogeneousMatrix &M)
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:106
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
The vpRotationMatrix considers the particular case of a rotation matrix.
virtual void init(const vpImage< unsigned char > &I)=0
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:485
virtual 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)=0
void savePose(const std::string &filename)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
The vpDisplayOpenCV allows to display image using the opencv library.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Class used for pose computation from N points (pose from point only).
Definition: vpPose.h:78
virtual void loadModel(const std::string &modelFile)
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.h:196
virtual void initCylinder(const vpPoint &p1, const vpPoint p2, const double radius, const unsigned int indexCylinder=0)=0
virtual void loadVRMLModel(const std::string &modelFile)
Defines a quaternion and its basic operations.
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, const bool displayHelp=false)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:368
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
std::string poseSavingFilename
Filename used to save the initial pose computed using the initClick() method. It is also used to read...
Definition: vpMbTracker.h:114
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:127
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:120
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
unsigned int getCols() const
Return the number of columns of the matrix.
Definition: vpMatrix.h:159
error that can be emited by the vpMatrix class and its derivates
void computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo)
compute the pose for a given method
Definition: vpPose.cpp:382
virtual bool getClick(bool blocking=true)=0
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
virtual void initFromPoints(const vpImage< unsigned char > &I, const std::string &initFile)
static void read(vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:277
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
void addPoint(const vpPoint &P)
Add a new point in this array.
Definition: vpPose.cpp:150
Class that consider the case of a translation vector.
virtual void displayPoint(const vpImagePoint &ip, const vpColor &color)=0
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the residual expressed in meter for the pose matrix 'cMo'.
Definition: vpPose.cpp:339
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74
void clearPoint()
suppress all the point in the array of point
Definition: vpPose.cpp:128
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, unsigned int &indexFace)