ViSP
vpWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * $Id: vpWireFrameSimulator.cpp 5297 2015-02-10 11:19:24Z 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  *
34  * Description:
35  * Wire frame simulator
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
47 #include <visp/vpWireFrameSimulator.h>
48 #include <fcntl.h>
49 #include <string.h>
50 #include <stdio.h>
51 #include <vector>
52 
53 #include <visp/vpSimulatorException.h>
54 #include <visp/vpPoint.h>
55 #include <visp/vpCameraParameters.h>
56 #include <visp/vpMeterPixelConversion.h>
57 #include <visp/vpPoint.h>
58 #include <visp/vpIoTools.h>
59 
60 //Inventor includes
61 #if defined(VISP_HAVE_COIN)
62 #include <Inventor/nodes/SoSeparator.h>
63 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
64 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
65 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
66 #include <Inventor/actions/SoWriteAction.h>
67 #include <Inventor/actions/SoSearchAction.h>
68 #include <Inventor/misc/SoChildList.h>
69 #include <Inventor/actions/SoGetMatrixAction.h>
70 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
71 #include <Inventor/actions/SoToVRML2Action.h>
72 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
73 #include <Inventor/VRMLnodes/SoVRMLShape.h>
74 #endif
75 
76 extern Point2i *point2i;
77 extern Point2i *listpoint2i;
78 
79 typedef enum
80 {
81  BND_MODEL,
82  WRL_MODEL,
83  UNKNOWN_MODEL
84 } Model_3D;
85 
86 Model_3D getExtension(const char* file);
87 void set_scene_wrl (const char* str, Bound_scene *sc, float factor);
88 
89 /*
90  Get the extension of the file and return it
91 */
92 Model_3D
93 getExtension(const char* file)
94 {
95  std::string sfilename(file);
96 
97  size_t bnd = sfilename.find("bnd");
98  size_t BND = sfilename.find("BND");
99  size_t wrl = sfilename.find("wrl");
100  size_t WRL = sfilename.find("WRL");
101 
102  size_t size = sfilename.size();
103 
104  if ((bnd>0 && bnd<size ) || (BND>0 && BND<size))
105  return BND_MODEL;
106  else if ((wrl>0 && wrl<size) || ( WRL>0 && WRL<size))
107  {
108 #if defined(VISP_HAVE_COIN)
109  return WRL_MODEL;
110 #else
111  std::cout << "Coin not installed, cannot read VRML files" << std::endl;
112  throw std::string("Coin not installed, cannot read VRML files");
113 #endif
114  }
115  return UNKNOWN_MODEL;
116 }
117 
118 /*
119  Enable to initialize the scene
120 */
121 void set_scene (const char* str, Bound_scene *sc, float factor)
122 {
123  FILE *fd;
124 
125  //if ((fd = fopen (str, 0)) == -1)
126  if ((fd = fopen (str, "r")) == NULL)
127  {
128  std::string error = "The file " + std::string(str) + " can not be opened";
129 
130  throw(vpException(vpSimulatorException::ioError, error.c_str())) ;
131  }
132  open_keyword (keyword_tbl);
133  open_lex ();
134  open_source (fd, str);
135  malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
136  parser (sc);
137 
138  //if (factor != 1)
139  if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
140  {
141  for (int i = 0; i < sc->bound.nbr; i++)
142  {
143  for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
144  {
145  sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
146  sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
147  sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
148  }
149  }
150  }
151 
152  close_source ();
153  close_lex ();
154  close_keyword ();
155  fclose(fd);
156 }
157 
158 #if defined(VISP_HAVE_COIN)
159 
160 #ifndef DOXYGEN_SHOULD_SKIP_THIS
161 typedef struct indexFaceSet
162 {
163  indexFaceSet() : nbPt(0), pt(), nbIndex(0), index() {};
164  int nbPt;
165  std::vector<vpPoint> pt;
166  int nbIndex;
167  std::vector<int> index;
168 } indexFaceSet;
169 #endif // DOXYGEN_SHOULD_SKIP_THIS
170 
171 void extractFaces(SoVRMLIndexedFaceSet*, indexFaceSet *ifs);
172 void ifsToBound (Bound*, std::list<indexFaceSet*> &);
173 void destroyIfs(std::list<indexFaceSet*> &);
174 
175 
176 void set_scene_wrl (const char* str, Bound_scene *sc, float factor)
177 {
178  //Load the sceneGraph
179  SoDB::init();
180  SoInput in;
181  SbBool ok = in.openFile(str);
182  SoSeparator *sceneGraph;
183  SoVRMLGroup *sceneGraphVRML2;
184 
185  if (!ok) {
186  vpERROR_TRACE("can't open file \"%s\" \n Please check the Marker_Less.ini file", str);
187  exit(1);
188  }
189 
190  if(!in.isFileVRML2())
191  {
192  sceneGraph = SoDB::readAll(&in);
193  if (sceneGraph == NULL) { /*return -1;*/ }
194  sceneGraph->ref();
195 
196  SoToVRML2Action tovrml2;
197  tovrml2.apply(sceneGraph);
198  sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
199  sceneGraphVRML2->ref();
200  sceneGraph->unref();
201  }
202  else
203  {
204  sceneGraphVRML2 = SoDB::readAllVRML(&in);
205  if (sceneGraphVRML2 == NULL) {
206  /*return -1;*/
207  throw(vpException(vpException::notInitialized, "Cannot read VRML file"));
208  }
209  sceneGraphVRML2->ref();
210  }
211 
212  in.closeFile();
213 
214  int nbShapes = sceneGraphVRML2->getNumChildren();
215 
216  SoNode * child;
217 
218  malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
219 
220  int iterShapes = 0;
221  for (int i = 0; i < nbShapes; i++)
222  {
223  int nbFaces = 0;
224  child = sceneGraphVRML2->getChild(i);
225  if (child->getTypeId() == SoVRMLShape::getClassTypeId())
226  {
227  std::list<indexFaceSet*> ifs_list;
228  SoChildList * child2list = child->getChildren();
229  for (int j = 0; j < child2list->getLength(); j++)
230  {
231  if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
232  {
233  indexFaceSet *ifs = new indexFaceSet;
234  SoVRMLIndexedFaceSet * face_set;
235  face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
236  extractFaces(face_set,ifs);
237  ifs_list.push_back(ifs);
238  nbFaces++;
239  }
240 // if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
241 // {
242 // std::cout << "> We found a line" << std::endl;
243 // SoVRMLIndexedLineSet * line_set;
244 // line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
245 // extractLines(line_set);
246 // }
247  }
248  sc->bound.nbr++;
249  ifsToBound (&(sc->bound.ptr[iterShapes]), ifs_list);
250  destroyIfs(ifs_list);
251  iterShapes++;
252  }
253  }
254 
255  //if (factor != 1)
256  if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
257  {
258  for (int i = 0; i < sc->bound.nbr; i++)
259  {
260  for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
261  {
262  sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
263  sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
264  sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
265  }
266  }
267  }
268 }
269 
270 
271 void
272 extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs)
273 {
274 // vpList<vpPoint> pointList;
275 // pointList.kill();
276  SoVRMLCoordinate *coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
277  int coordSize = coord->point.getNum();
278 
279  ifs->nbPt = coordSize;
280  for (int i = 0; i < coordSize; i++)
281  {
282  SbVec3f point(0,0,0);
283  point[0]=coord->point[i].getValue()[0];
284  point[1]=coord->point[i].getValue()[1];
285  point[2]=coord->point[i].getValue()[2];
286  vpPoint pt;
287  pt.setWorldCoordinates(point[0],point[1],point[2]);
288  ifs->pt.push_back(pt);
289  }
290 
291  SoMFInt32 indexList = face_set->coordIndex;
292  int indexListSize = indexList.getNum();
293 
294  ifs->nbIndex = indexListSize;
295  for (int i = 0; i < indexListSize; i++)
296  {
297  int index = face_set->coordIndex[i];
298  ifs->index.push_back(index);
299  }
300 }
301 
302 void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
303 {
304  int nbPt = 0;
305  for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
306  nbPt += (*it)->nbPt;
307  }
308  bptr->point.nbr = (Index)nbPt;
309  bptr->point.ptr = (Point3f *) malloc ((unsigned int)nbPt * sizeof (Point3f));
310 
311  ifs_list.front();
312  unsigned int iter = 0;
313  for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
314  {
315  indexFaceSet* ifs = *it;
316  for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++)
317  {
318  bptr->point.ptr[iter].x = (float)ifs->pt[j].get_oX();
319  bptr->point.ptr[iter].y = (float)ifs->pt[j].get_oY();
320  bptr->point.ptr[iter].z = (float)ifs->pt[j].get_oZ();
321  iter++;
322  }
323  }
324 
325  unsigned int nbFace = 0;
326  ifs_list.front();
327  std::list<int> indSize;
328  int indice = 0;
329  for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
330  {
331  indexFaceSet* ifs = *it;
332  for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
333  {
334  if(ifs->index[j] == -1)
335  {
336  nbFace++;
337  indSize.push_back(indice);
338  indice = 0;
339  }
340  else indice++;
341  }
342  }
343 
344  bptr->face.nbr = (Index)nbFace;
345  bptr->face.ptr = (Face *) malloc (nbFace * sizeof (Face));
346 
347 
348  std::list<int>::const_iterator iter_indSize = indSize.begin();
349  for (unsigned int i = 0; i < indSize.size(); i++)
350  {
351  bptr->face.ptr[i].vertex.nbr = (Index)*iter_indSize;
352  bptr->face.ptr[i].vertex.ptr = (Index *) malloc ((unsigned int)*iter_indSize * sizeof (Index));
353  ++iter_indSize;
354  }
355 
356  int offset = 0;
357  indice = 0;
358  for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
359  {
360  indexFaceSet* ifs = *it;
361  iter = 0;
362  for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
363  {
364  if(ifs->index[j] != -1)
365  {
366  bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset);
367  iter++;
368  }
369  else
370  {
371  iter = 0;
372  indice++;
373  }
374  }
375  offset += ifs->nbPt;
376  }
377 }
378 
379 void destroyIfs(std::list<indexFaceSet*> &ifs_list)
380 {
381  for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
382  delete *it;
383  }
384  ifs_list.clear();
385 }
386 #else
387 void set_scene_wrl (const char* /*str*/, Bound_scene* /*sc*/, float /*factor*/)
388 {
389 }
390 #endif
391 
392 
393 /*
394  Convert the matrix format to deal with the one in the simulator
395 */
396 void vp2jlc_matrix (const vpHomogeneousMatrix vpM, Matrix &jlcM)
397 {
398  for (unsigned int i = 0; i < 4; i++) {
399  for (unsigned int j = 0; j < 4; j++)
400  jlcM[j][i] = (float)vpM[i][j];
401  }
402 }
403 
404 /*
405  Copy the scene corresponding to the registeresd parameters in the image.
406 */
407 void
408 vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
409 {
410  // extern Bound *clipping_Bound ();
411  Bound *bp, *bend;
412  Bound *clip; /* surface apres clipping */
413  Byte b = (Byte) *get_rfstack ();
414  Matrix m;
415 
416  //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
417  memmove((char *) m, (char *) mat, sizeof (Matrix));
418  View_to_Matrix (get_vwstack (), *(get_tmstack ()));
419  postmult_matrix (m, *(get_tmstack ()));
420  bp = sc.bound.ptr;
421  bend = bp + sc.bound.nbr;
422  for (; bp < bend; bp++)
423  {
424  if ((clip = clipping_Bound (bp, m)) != NULL)
425  {
426  Face *fp = clip->face.ptr;
427  Face *fend = fp + clip->face.nbr;
428 
429  set_Bound_face_display (clip, b); //regarde si is_visible
430 
431  point_3D_2D (clip->point.ptr, clip->point.nbr,I.getWidth(),I.getHeight(),point2i);
432  for (; fp < fend; fp++)
433  {
434  if (fp->is_visible)
435  {
436  wireframe_Face (fp, point2i);
437  Point2i *pt = listpoint2i;
438  for (int i = 1; i < fp->vertex.nbr; i++)
439  {
440  vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
441  pt++;
442  }
443  if (fp->vertex.nbr > 2)
444  {
445  vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
446  }
447  }
448  }
449  }
450  }
451 }
452 
453 /*
454  Copy the scene corresponding to the registeresd parameters in the image.
455 */
456 void
457 vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color)
458 {
459  //extern Bound *clipping_Bound ();
460 
461  Bound *bp, *bend;
462  Bound *clip; /* surface apres clipping */
463  Byte b = (Byte) *get_rfstack ();
464  Matrix m;
465 
466  //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
467  memmove((char *) m, (char *) mat, sizeof (Matrix));
468  View_to_Matrix (get_vwstack (), *(get_tmstack ()));
469  postmult_matrix (m, *(get_tmstack ()));
470  bp = sc.bound.ptr;
471  bend = bp + sc.bound.nbr;
472  for (; bp < bend; bp++)
473  {
474  if ((clip = clipping_Bound (bp, m)) != NULL)
475  {
476  Face *fp = clip->face.ptr;
477  Face *fend = fp + clip->face.nbr;
478 
479  set_Bound_face_display (clip, b); //regarde si is_visible
480 
481  point_3D_2D (clip->point.ptr, clip->point.nbr,I.getWidth(),I.getHeight(),point2i);
482  for (; fp < fend; fp++)
483  {
484  if (fp->is_visible)
485  {
486  wireframe_Face (fp, point2i);
487  Point2i *pt = listpoint2i;
488  for (int i = 1; i < fp->vertex.nbr; i++)
489  {
490  vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
491  pt++;
492  }
493  if (fp->vertex.nbr > 2)
494  {
495  vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
496  }
497  }
498  }
499  }
500  }
501 }
502 
503 /*************************************************************************************************************/
504 
514  : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(),
515  refMo(), cMo(), cdMo(), object(PLATE), desiredObject(D_STANDARD),
516  camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
517  desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true),
518  cameraTrajectory(), poseList(), fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(),
519  old_iPt(), blockedr(false), blockedz(false), blockedt(false), blocked(false),
520  camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
521  displayDesiredObject(false), displayCamera(false), displayImageSimulator(false),
522  cameraFactor(1.), camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
523 {
524  // set scene_dir from #define VISP_SCENE_DIR if it exists
525  // VISP_SCENES_DIR may contain multiple locations separated by ";"
526  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
527  bool sceneDirExists = false;
528  for(size_t i=0; i < scene_dirs.size(); i++)
529  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
530  scene_dir = scene_dirs[i];
531  sceneDirExists = true;
532  break;
533  }
534  if (! sceneDirExists) {
535  try {
536  scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
537  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
538  }
539  catch (...) {
540  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
541  }
542  }
543 
544  open_display();
545  open_clipping();
546 
547  old_iPr = vpImagePoint(-1,-1);
548  old_iPz = vpImagePoint(-1,-1);
549  old_iPt = vpImagePoint(-1,-1);
550 
551  rotz.buildFrom(0,0,0,0,0,vpMath::rad(180));
552 
553  scene.name = NULL;
554  scene.bound.ptr = NULL;
555  scene.bound.nbr = 0;
556 
557  desiredScene.name = NULL;
558  desiredScene.bound.ptr = NULL;
559  desiredScene.bound.nbr = 0;
560 
561  camera.name = NULL;
562  camera.bound.ptr = NULL;
563  camera.bound.nbr = 0;
564 }
565 
566 
571 {
572  if(sceneInitialized)
573  {
574  if(displayObject)
575  free_Bound_scene (&(this->scene));
576  if(displayCamera){
577  free_Bound_scene (&(this->camera));
578  }
580  free_Bound_scene (&(this->desiredScene));
581  }
582  close_clipping();
583  close_display ();
584 
585  cameraTrajectory.clear();
586  poseList.clear();
587  fMoList.clear();
588 }
589 
590 
600 void
602 {
603  char name_cam[FILENAME_MAX];
604  char name[FILENAME_MAX];
605 
606  object = obj;
607  this->desiredObject = desired_object;
608 
609  const char *scene_dir_ = scene_dir.c_str();
610  if (strlen(scene_dir_) >= FILENAME_MAX) {
612  "Not enough memory to intialize the camera name"));
613  }
614 
615  strcpy(name_cam, scene_dir_);
616  if (desiredObject != D_TOOL)
617  {
618  strcat(name_cam,"/camera.bnd");
619  set_scene(name_cam,&camera,cameraFactor);
620  }
621  else
622  {
623  strcat(name_cam,"/tool.bnd");
624  set_scene(name_cam,&(this->camera),1.0);
625  }
626 
627  strcpy(name, scene_dir_);
628  switch (obj)
629  {
630  case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
631  case CUBE : { strcat(name, "/cube.bnd"); break; }
632  case PLATE : { strcat(name, "/plate.bnd"); break; }
633  case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
634  case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
635  case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
636  case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
637  case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
638  case THREE_LINES : { strcat(name, "/line.bnd"); break; }
639  case ROAD : { strcat(name, "/road.bnd"); break; }
640  case TIRE : { strcat(name, "/circles2.bnd"); break; }
641  case PIPE : { strcat(name, "/pipe.bnd"); break; }
642  case CIRCLE : { strcat(name, "/circle.bnd"); break; }
643  case SPHERE : { strcat(name, "/sphere.bnd"); break; }
644  case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
645  case PLAN: { strcat(name, "/plan.bnd"); break; }
646  case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
647  }
648  set_scene(name,&(this->scene),1.0);
649 
650  scene_dir_ = scene_dir.c_str();
651  if (strlen(scene_dir_) >= FILENAME_MAX) {
653  "Not enough memory to intialize the desired object name"));
654  }
655 
656  switch (desiredObject)
657  {
658  case D_STANDARD : { break; }
659  case D_CIRCLE : {
660  strcpy(name, scene_dir_);
661  strcat(name, "/circle_sq2.bnd");
662  break; }
663  case D_TOOL : {
664  strcpy(name, scene_dir_);
665  strcat(name, "/tool.bnd");
666  break; }
667  }
668  set_scene(name, &(this->desiredScene), 1.0);
669 
670  if (obj == PIPE) load_rfstack(IS_INSIDE);
671  else add_rfstack(IS_BACK);
672 
673  add_vwstack ("start","depth", 0.0, 100.0);
674  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
675  add_vwstack ("start","type", PERSPECTIVE);
676 
677  sceneInitialized = true;
678  displayObject = true;
679  displayDesiredObject = true;
680  displayCamera = true;
681  displayImageSimulator = true;
682 }
683 
684 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
685 
701 void
703 {
704  initScene(obj, desired_object);
705  objectImage.clear();
706  for(imObj.front(); !imObj.outside(); imObj.next()){
707  objectImage.push_back(imObj.value());
708  }
709  displayImageSimulator = true;
710 }
711 #endif
712 
725 void
726 vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object, const std::list<vpImageSimulator> &imObj)
727 {
728  initScene(obj, desired_object);
729  objectImage = imObj;
730  displayImageSimulator = true;
731 }
732 
733 
743 void
744 vpWireFrameSimulator::initScene(const char* obj, const char* desired_object)
745 {
746  char name_cam[FILENAME_MAX];
747  char name[FILENAME_MAX];
748 
749  object = THREE_PTS;
750  this->desiredObject = D_STANDARD;
751 
752  const char *scene_dir_ = scene_dir.c_str();
753  if (strlen(scene_dir_) >= FILENAME_MAX) {
755  "Not enough memory to intialize the camera name"));
756  }
757 
758  strcpy(name_cam, scene_dir_);
759  strcat(name_cam,"/camera.bnd");
760  set_scene(name_cam,&camera,cameraFactor);
761 
762  if (strlen(obj) >= FILENAME_MAX) {
764  "Not enough memory to intialize the name"));
765  }
766 
767  strcpy(name,obj);
768  Model_3D model;
769  model = getExtension(obj);
770  if (model == BND_MODEL)
771  set_scene(name,&(this->scene),1.0);
772  else if (model == WRL_MODEL)
773  set_scene_wrl(name,&(this->scene),1.0);
774  else if (model == UNKNOWN_MODEL)
775  {
776  vpERROR_TRACE("Unknown file extension for the 3D model");
777  }
778 
779  if (strlen(desired_object) >= FILENAME_MAX) {
781  "Not enough memory to intialize the camera name"));
782  }
783 
784  strcpy(name,desired_object);
785  model = getExtension(desired_object);
786  if (model == BND_MODEL)
787  set_scene(name,&(this->desiredScene),1.0);
788  else if (model == WRL_MODEL)
789  set_scene_wrl(name,&(this->desiredScene),1.0);
790  else if (model == UNKNOWN_MODEL)
791  {
792  vpERROR_TRACE("Unknown file extension for the 3D model");
793  }
794 
795  add_rfstack(IS_BACK);
796 
797  add_vwstack ("start","depth", 0.0, 100.0);
798  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
799  add_vwstack ("start","type", PERSPECTIVE);
800 
801  sceneInitialized = true;
802  displayObject = true;
803  displayDesiredObject = true;
804  displayCamera = true;
805 }
806 
807 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
808 
823 void
824 vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, vpList<vpImageSimulator> &imObj)
825 {
826  initScene(obj, desired_object);
827  objectImage.clear();
828  for(imObj.front(); !imObj.outside(); imObj.next()){
829  objectImage.push_back(imObj.value());
830  }
831  displayImageSimulator = true;
832 }
833 #endif
834 
847 void
848 vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, const std::list<vpImageSimulator> &imObj)
849 {
850  initScene(obj, desired_object);
851  objectImage = imObj;
852  displayImageSimulator = true;
853 }
854 
863 void
865 {
866  char name_cam[FILENAME_MAX];
867  char name[FILENAME_MAX];
868 
869  object = obj;
870 
871  const char *scene_dir_ = scene_dir.c_str();
872  if (strlen(scene_dir_) >= FILENAME_MAX) {
874  "Not enough memory to intialize the camera name"));
875  }
876 
877  strcpy(name_cam, scene_dir_);
878  strcat(name_cam,"/camera.bnd");
879  set_scene(name_cam,&camera,cameraFactor);
880 
881  strcpy(name, scene_dir_);
882  switch (obj)
883  {
884  case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
885  case CUBE : { strcat(name, "/cube.bnd"); break; }
886  case PLATE : { strcat(name, "/plate.bnd"); break; }
887  case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
888  case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
889  case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
890  case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
891  case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
892  case THREE_LINES : { strcat(name, "/line.bnd"); break; }
893  case ROAD : { strcat(name, "/road.bnd"); break; }
894  case TIRE : { strcat(name, "/circles2.bnd"); break; }
895  case PIPE : { strcat(name, "/pipe.bnd"); break; }
896  case CIRCLE : { strcat(name, "/circle.bnd"); break; }
897  case SPHERE : { strcat(name, "/sphere.bnd"); break; }
898  case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
899  case PLAN: { strcat(name, "/plan.bnd"); break; }
900  case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
901  }
902  set_scene(name,&(this->scene),1.0);
903 
904  if (obj == PIPE) load_rfstack(IS_INSIDE);
905  else add_rfstack(IS_BACK);
906 
907  add_vwstack ("start","depth", 0.0, 100.0);
908  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
909  add_vwstack ("start","type", PERSPECTIVE);
910 
911  sceneInitialized = true;
912  displayObject = true;
913  displayCamera = true;
914 
915  displayDesiredObject = false;
916  displayImageSimulator = false;
917 }
918 
919 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
920 
934 void
936 {
937  initScene(obj);
938  objectImage.clear();
939  for(imObj.front(); !imObj.outside(); imObj.next()){
940  objectImage.push_back(imObj.value());
941  }
942  displayImageSimulator = true;
943 }
944 #endif
945 
957 void
958 vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
959 {
960  initScene(obj);
961  objectImage = imObj;
962  displayImageSimulator = true;
963 }
964 
973 void
975 {
976  char name_cam[FILENAME_MAX];
977  char name[FILENAME_MAX];
978 
979  object = THREE_PTS;
980 
981  const char *scene_dir_ = scene_dir.c_str();
982  if (strlen(scene_dir_) >= FILENAME_MAX) {
984  "Not enough memory to intialize the camera name"));
985  }
986 
987  strcpy(name_cam, scene_dir_);
988  strcat(name_cam,"/camera.bnd");
989  set_scene(name_cam,&camera,cameraFactor);
990 
991  if (strlen(obj) >= FILENAME_MAX) {
993  "Not enough memory to intialize the name"));
994  }
995 
996  strcpy(name,obj);
997  Model_3D model;
998  model = getExtension(obj);
999  if (model == BND_MODEL)
1000  set_scene(name,&(this->scene),1.0);
1001  else if (model == WRL_MODEL)
1002  set_scene_wrl(name,&(this->scene),1.0);
1003  else if (model == UNKNOWN_MODEL)
1004  {
1005  vpERROR_TRACE("Unknown file extension for the 3D model");
1006  }
1007 
1008  add_rfstack(IS_BACK);
1009 
1010  add_vwstack ("start","depth", 0.0, 100.0);
1011  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
1012  add_vwstack ("start","type", PERSPECTIVE);
1013 
1014  sceneInitialized = true;
1015  displayObject = true;
1016  displayCamera = true;
1017 }
1018 
1019 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1020 
1034 void
1036 {
1037  initScene(obj);
1038  objectImage.clear();
1039  for(imObj.front(); !imObj.outside(); imObj.next()){
1040  objectImage.push_back(imObj.value());
1041  }
1042  displayImageSimulator = true;
1043 }
1044 #endif
1045 
1057 void
1058 vpWireFrameSimulator::initScene(const char* obj, const std::list<vpImageSimulator> &imObj)
1059 {
1060  initScene(obj);
1061  objectImage = imObj;
1062  displayImageSimulator = true;
1063 }
1064 
1072 void
1074 {
1075  if (!sceneInitialized)
1076  throw(vpException(vpSimulatorException::notInitializedError,"The scene has to be initialized")) ;
1077 
1078  double u;
1079  double v;
1080  //if(px_int != 1 && py_int != 1)
1081  // we assume px_int and py_int > 0
1082  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
1083  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
1084  {
1085  u = (double)I.getWidth()/(2*px_int);
1086  v = (double)I.getHeight()/(2*py_int);
1087  }
1088  else
1089  {
1090  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1091  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1092  }
1093 
1094  float o44c[4][4],o44cd[4][4],x,y,z;
1095  Matrix id = IDENTITY_MATRIX;
1096 
1097  vp2jlc_matrix(cMo.inverse(),o44c);
1098  vp2jlc_matrix(cdMo.inverse(),o44cd);
1099 
1101  {
1102  I = 255;
1103 
1104  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1105  vpImageSimulator* imSim = &(*it);
1106  imSim->setCameraPosition(rotz*cMo);
1108  }
1109 
1110  if (I.display != NULL)
1111  vpDisplay::display(I);
1112  }
1113 
1114  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
1115  x = o44c[2][0] + o44c[3][0];
1116  y = o44c[2][1] + o44c[3][1];
1117  z = o44c[2][2] + o44c[3][2];
1118  add_vwstack ("start","vrp", x,y,z);
1119  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
1120  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
1121  add_vwstack ("start","window", -u, u, -v, v);
1122  if (displayObject)
1123  display_scene(id,this->scene,I, curColor);
1124 
1125 
1126  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
1127  x = o44cd[2][0] + o44cd[3][0];
1128  y = o44cd[2][1] + o44cd[3][1];
1129  z = o44cd[2][2] + o44cd[3][2];
1130  add_vwstack ("start","vrp", x,y,z);
1131  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
1132  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
1133  add_vwstack ("start","window", -u, u, -v, v);
1135  {
1137  else display_scene(id,desiredScene,I, desColor);
1138  }
1139 }
1140 
1141 
1150 void
1152 {
1153  bool changed = false;
1154  vpHomogeneousMatrix displacement = navigation(I,changed);
1155 
1156  //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1157  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1158  camMf2 = camMf2*displacement;
1159 
1160  f2Mf = camMf2.inverse()*camMf;
1161 
1162  camMf = camMf2* displacement * f2Mf;
1163 
1164  double u;
1165  double v;
1166  //if(px_ext != 1 && py_ext != 1)
1167  // we assume px_ext and py_ext > 0
1168  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
1169  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
1170  {
1171  u = (double)I.getWidth()/(2*px_ext);
1172  v = (double)I.getHeight()/(2*py_ext);
1173  }
1174  else
1175  {
1176  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1177  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1178  }
1179 
1180  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
1181 
1182  vp2jlc_matrix(camMf.inverse(),w44cext);
1183  vp2jlc_matrix(fMc,w44c);
1184  vp2jlc_matrix(fMo,w44o);
1185 
1186 
1187  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
1188  x = w44cext[2][0] + w44cext[3][0];
1189  y = w44cext[2][1] + w44cext[3][1];
1190  z = w44cext[2][2] + w44cext[3][2];
1191  add_vwstack ("start","vrp", x,y,z);
1192  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
1193  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
1194  add_vwstack ("start","window", -u, u, -v, v);
1195  if ((object == CUBE) || (object == SPHERE))
1196  {
1197  add_vwstack ("start","type", PERSPECTIVE);
1198  }
1199 
1201  {
1202  I = 255;
1203 
1204  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1205  vpImageSimulator* imSim = &(*it);
1206  imSim->setCameraPosition(rotz*camMf*fMo);
1208  }
1209 
1210  if (I.display != NULL)
1211  vpDisplay::display(I);
1212  }
1213 
1214  if (displayObject)
1215  display_scene(w44o,this->scene,I, curColor);
1216 
1217  if (displayCamera)
1218  display_scene(w44c,camera, I, camColor);
1219 
1221  {
1222  vpImagePoint iP;
1223  vpImagePoint iP_1;
1224  poseList.push_back(cMo);
1225  fMoList.push_back(fMo);
1226 
1227  int iter = 0;
1228 
1229  if (changed || extCamChanged)
1230  {
1231  cameraTrajectory.clear();
1232  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
1233  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
1234 
1235  while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end()))
1236  {
1237  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
1238  cameraTrajectory.push_back(iP);
1239  if (camTrajType == CT_LINE)
1240  {
1241  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
1242  }
1243  else if (camTrajType == CT_POINT)
1245  ++iter_poseList;
1246  ++iter_fMoList;
1247  iter++;
1248  iP_1 = iP;
1249  }
1250  extCamChanged = false;
1251  }
1252  else
1253  {
1254  iP = projectCameraTrajectory(I, cMo, fMo);
1255  cameraTrajectory.push_back(iP);
1256 
1257  for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
1258  if (camTrajType == CT_LINE)
1259  {
1260  if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
1261  }
1262  else if (camTrajType == CT_POINT)
1264  iter++;
1265  iP_1 = *it;
1266  }
1267  }
1268 
1269  if (poseList.size() > nbrPtLimit)
1270  {
1271  poseList.pop_front();
1272  }
1273  if (fMoList.size() > nbrPtLimit)
1274  {
1275  fMoList.pop_front();
1276  }
1277  if (cameraTrajectory.size() > nbrPtLimit)
1278  {
1279  cameraTrajectory.pop_front();
1280  }
1281  }
1282 }
1283 
1284 
1293 void
1295 {
1296  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
1297 
1298  vpHomogeneousMatrix camMft = rotz * cam_Mf;
1299 
1300  double u;
1301  double v;
1302  //if(px_ext != 1 && py_ext != 1)
1303  // we assume px_ext and py_ext > 0
1304  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
1305  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
1306  {
1307  u = (double)I.getWidth()/(2*px_ext);
1308  v = (double)I.getHeight()/(2*py_ext);
1309  }
1310  else
1311  {
1312  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1313  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1314  }
1315 
1316  vp2jlc_matrix(camMft.inverse(),w44cext);
1317  vp2jlc_matrix(fMo*cMo.inverse(),w44c);
1318  vp2jlc_matrix(fMo,w44o);
1319 
1320  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
1321  x = w44cext[2][0] + w44cext[3][0];
1322  y = w44cext[2][1] + w44cext[3][1];
1323  z = w44cext[2][2] + w44cext[3][2];
1324  add_vwstack ("start","vrp", x,y,z);
1325  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
1326  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
1327  add_vwstack ("start","window", -u, u, -v, v);
1328 
1330  {
1331  I = 255;
1332 
1333  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1334  vpImageSimulator* imSim = &(*it);
1335  imSim->setCameraPosition(rotz*cam_Mf*fMo);
1337  }
1338 
1339  if (I.display != NULL)
1340  vpDisplay::display(I);
1341  }
1342 
1343  if (displayObject)
1344  display_scene(w44o,this->scene,I, curColor);
1345  if (displayCamera)
1346  display_scene(w44c,camera, I, camColor);
1347 }
1348 
1349 
1357 void
1359 {
1360  if (!sceneInitialized)
1361  throw(vpException(vpSimulatorException::notInitializedError,"The scene has to be initialized")) ;
1362 
1363  double u;
1364  double v;
1365  //if(px_int != 1 && py_int != 1)
1366  // we assume px_int and py_int > 0
1367  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
1368  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
1369  {
1370  u = (double)I.getWidth()/(2*px_int);
1371  v = (double)I.getHeight()/(2*py_int);
1372  }
1373  else
1374  {
1375  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1376  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1377  }
1378 
1379  float o44c[4][4],o44cd[4][4],x,y,z;
1380  Matrix id = IDENTITY_MATRIX;
1381 
1382  vp2jlc_matrix(cMo.inverse(),o44c);
1383  vp2jlc_matrix(cdMo.inverse(),o44cd);
1384 
1386  {
1387  I = 255;
1388 
1389  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1390  vpImageSimulator* imSim = &(*it);
1391  imSim->setCameraPosition(rotz*camMf*fMo);
1393  }
1394 
1395  if (I.display != NULL)
1396  vpDisplay::display(I);
1397  }
1398 
1399  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
1400  x = o44c[2][0] + o44c[3][0];
1401  y = o44c[2][1] + o44c[3][1];
1402  z = o44c[2][2] + o44c[3][2];
1403  add_vwstack ("start","vrp", x,y,z);
1404  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
1405  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
1406  add_vwstack ("start","window", -u, u, -v, v);
1407  if (displayObject)
1408  display_scene(id,this->scene,I, curColor);
1409 
1410 
1411  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
1412  x = o44cd[2][0] + o44cd[3][0];
1413  y = o44cd[2][1] + o44cd[3][1];
1414  z = o44cd[2][2] + o44cd[3][2];
1415  add_vwstack ("start","vrp", x,y,z);
1416  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
1417  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
1418  add_vwstack ("start","window", -u, u, -v, v);
1420  {
1422  else display_scene(id,desiredScene,I, desColor);
1423  }
1424 }
1425 
1426 
1435 void
1437 {
1438  bool changed = false;
1439  vpHomogeneousMatrix displacement = navigation(I,changed);
1440 
1441  //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1442  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1443  camMf2 = camMf2*displacement;
1444 
1445  f2Mf = camMf2.inverse()*camMf;
1446 
1447  camMf = camMf2* displacement * f2Mf;
1448 
1449  double u;
1450  double v;
1451  //if(px_ext != 1 && py_ext != 1)
1452  // we assume px_ext and py_ext > 0
1453  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
1454  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
1455  {
1456  u = (double)I.getWidth()/(2*px_ext);
1457  v = (double)I.getHeight()/(2*py_ext);
1458  }
1459  else
1460  {
1461  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1462  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1463  }
1464 
1465  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
1466 
1467  vp2jlc_matrix(camMf.inverse(),w44cext);
1468  vp2jlc_matrix(fMc,w44c);
1469  vp2jlc_matrix(fMo,w44o);
1470 
1471 
1472  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
1473  x = w44cext[2][0] + w44cext[3][0];
1474  y = w44cext[2][1] + w44cext[3][1];
1475  z = w44cext[2][2] + w44cext[3][2];
1476  add_vwstack ("start","vrp", x,y,z);
1477  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
1478  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
1479  add_vwstack ("start","window", -u, u, -v, v);
1480  if ((object == CUBE) || (object == SPHERE))
1481  {
1482  add_vwstack ("start","type", PERSPECTIVE);
1483  }
1484 
1486  {
1487  I = 255;
1488  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1489  vpImageSimulator* imSim = &(*it);
1490  imSim->setCameraPosition(rotz*camMf*fMo);
1492  }
1493  if (I.display != NULL)
1494  vpDisplay::display(I);
1495  }
1496 
1497  if (displayObject)
1498  display_scene(w44o,this->scene,I, curColor);
1499 
1500  if (displayCamera)
1501  display_scene(w44c,camera, I, camColor);
1502 
1504  {
1505  vpImagePoint iP;
1506  vpImagePoint iP_1;
1507  poseList.push_back(cMo);
1508  fMoList.push_back(fMo);
1509 
1510  int iter = 0;
1511 
1512  if (changed || extCamChanged)
1513  {
1514  cameraTrajectory.clear();
1515  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
1516  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
1517 
1518  while ((iter_poseList!=poseList.end()) && (iter_fMoList!=fMoList.end()) )
1519  {
1520  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
1521  cameraTrajectory.push_back(iP);
1522  //vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
1523  if (camTrajType == CT_LINE)
1524  {
1525  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
1526  }
1527  else if (camTrajType == CT_POINT)
1529  ++iter_poseList;
1530  ++iter_fMoList;
1531  iter++;
1532  iP_1 = iP;
1533  }
1534  extCamChanged = false;
1535  }
1536  else
1537  {
1538  iP = projectCameraTrajectory(I, cMo,fMo);
1539  cameraTrajectory.push_back(iP);
1540 
1541  for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
1542  if (camTrajType == CT_LINE){
1543  if (iter != 0) vpDisplay::displayLine(I,iP_1,*it,camTrajColor, thickness_);
1544  }
1545  else if(camTrajType == CT_POINT)
1547  iter++;
1548  iP_1 = *it;
1549  }
1550  }
1551 
1552  if (poseList.size() > nbrPtLimit)
1553  {
1554  poseList.pop_front();
1555  }
1556  if (fMoList.size() > nbrPtLimit)
1557  {
1558  fMoList.pop_front();
1559  }
1560  if (cameraTrajectory.size() > nbrPtLimit)
1561  {
1562  cameraTrajectory.pop_front();
1563  }
1564  }
1565 }
1566 
1567 
1576 void
1578 {
1579  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
1580 
1581  vpHomogeneousMatrix camMft = rotz * cam_Mf;
1582 
1583  double u;
1584  double v;
1585  //if(px_ext != 1 && py_ext != 1)
1586  // we assume px_ext and py_ext > 0
1587  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
1588  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
1589  {
1590  u = (double)I.getWidth()/(2*px_ext);
1591  v = (double)I.getHeight()/(2*py_ext);
1592  }
1593  else
1594  {
1595  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1596  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1597  }
1598 
1599  vp2jlc_matrix(camMft.inverse(),w44cext);
1600  vp2jlc_matrix(fMo*cMo.inverse(),w44c);
1601  vp2jlc_matrix(fMo,w44o);
1602 
1603  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
1604  x = w44cext[2][0] + w44cext[3][0];
1605  y = w44cext[2][1] + w44cext[3][1];
1606  z = w44cext[2][2] + w44cext[3][2];
1607  add_vwstack ("start","vrp", x,y,z);
1608  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
1609  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
1610  add_vwstack ("start","window", -u, u, -v, v);
1611 
1613  {
1614  I = 255;
1615  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1616  vpImageSimulator* imSim = &(*it);
1617  imSim->setCameraPosition(rotz*cam_Mf*fMo);
1619  }
1620  if (I.display != NULL)
1621  vpDisplay::display(I);
1622  }
1623 
1624  if (displayObject)
1625  display_scene(w44o,this->scene,I, curColor);
1626  if (displayCamera)
1627  display_scene(w44c,camera, I, camColor);
1628 }
1629 
1630 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1631 
1645 void
1647 {
1648  if (list_cMo.nbElements() != list_fMo.nbElements())
1649  throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
1650 
1651  list_cMo.front();
1652  list_fMo.front();
1653  vpImagePoint iP;
1654  vpImagePoint iP_1;
1655  int iter = 0;
1656 
1657  while (!list_cMo.outside() && !list_fMo.outside())
1658  {
1659  iP = projectCameraTrajectory(I, rotz * list_cMo.value(), list_fMo.value(), rotz * cMf);
1660  if (camTrajType == CT_LINE)
1661  {
1662  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
1663  }
1664  else if (camTrajType == CT_POINT)
1666  list_cMo.next();
1667  list_fMo.next();
1668  iter++;
1669  iP_1 = iP;
1670  }
1671 }
1672 
1687 void
1689 {
1690  if (list_cMo.nbElements() != list_fMo.nbElements())
1691  throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
1692 
1693  list_cMo.front();
1694  list_fMo.front();
1695  vpImagePoint iP;
1696  vpImagePoint iP_1;
1697  int iter = 0;
1698 
1699  while (!list_cMo.outside() && !list_fMo.outside())
1700  {
1701  iP = projectCameraTrajectory(I, rotz * list_cMo.value(), list_fMo.value(), rotz * cMf);
1702  if (camTrajType == CT_LINE)
1703  {
1704  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
1705  }
1706  else if (camTrajType == CT_POINT)
1708  list_cMo.next();
1709  list_fMo.next();
1710  iter++;
1711  iP_1 = iP;
1712  }
1713 }
1714 #endif
1715 
1716 
1727 void
1728 vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
1729  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
1730 {
1731  if (list_cMo.size() != list_fMo.size())
1732  throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
1733 
1734  vpImagePoint iP;
1735  vpImagePoint iP_1;
1736  int iter = 0;
1737 
1738  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1739  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1740 
1741  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
1742  {
1743  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1744  if (camTrajType == CT_LINE)
1745  {
1746  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
1747  }
1748  else if (camTrajType == CT_POINT)
1750  ++it_cMo;
1751  ++it_fMo;
1752  iter++;
1753  iP_1 = iP;
1754  }
1755 }
1756 
1767 void
1768 vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
1769  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
1770 {
1771  if (list_cMo.size() != list_fMo.size())
1772  throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
1773 
1774  vpImagePoint iP;
1775  vpImagePoint iP_1;
1776  int iter = 0;
1777 
1778  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1779  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1780 
1781  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
1782  {
1783  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1784  if (camTrajType == CT_LINE)
1785  {
1786  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
1787  }
1788  else if (camTrajType == CT_POINT)
1790  ++it_cMo;
1791  ++it_fMo;
1792  iter++;
1793  iP_1 = iP;
1794  }
1795 }
1796 
1797 
1803 {
1804  double width = vpMath::minimum(I.getWidth(),I.getHeight());
1805  vpImagePoint iP;
1806  vpImagePoint trash;
1807  bool clicked = false;
1808  bool clickedUp = false;
1810 
1811  vpHomogeneousMatrix mov(0,0,0,0,0,0);
1812  changed = false;
1813 
1814  //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1815 
1816  if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
1817 
1818  if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
1819 
1820  if(clicked)
1821  {
1822  if (b == vpMouseButton::button1) blockedr = true;
1823  if (b == vpMouseButton::button2) blockedz = true;
1824  if (b == vpMouseButton::button3) blockedt = true;
1825  blocked = true;
1826  }
1827  if(clickedUp)
1828  {
1829  if (b == vpMouseButton::button1)
1830  {
1831  old_iPr = vpImagePoint(-1,-1);
1832  blockedr = false;
1833  }
1834  if (b == vpMouseButton::button2)
1835  {
1836  old_iPz = vpImagePoint(-1,-1);
1837  blockedz = false;
1838  }
1839  if (b == vpMouseButton::button3)
1840  {
1841  old_iPt = vpImagePoint(-1,-1);
1842  blockedt = false;
1843  }
1844  if (!(blockedr || blockedz || blockedt))
1845  {
1846  blocked = false;
1847  while (vpDisplay::getClick(I,trash,b,false)) {};
1848  }
1849  }
1850 
1852 
1853  double anglei = 0;
1854  double anglej = 0;
1855 
1856  if (old_iPr != vpImagePoint(-1,-1) && blockedr)
1857  {
1858  double diffi = iP.get_i() - old_iPr.get_i();
1859  double diffj = iP.get_j() - old_iPr.get_j();
1860  //cout << "delta :" << diffj << endl;;
1861  anglei = diffi*360/width;
1862  anglej = diffj*360/width;
1863  mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
1864  changed = true;
1865  }
1866 
1867  if (blockedr) old_iPr = iP;
1868 
1869  if (old_iPz != vpImagePoint(-1,-1) && blockedz)
1870  {
1871  double diffi = iP.get_i() - old_iPz.get_i();
1872  mov.buildFrom(0,0,diffi*0.01,0,0,0);
1873  changed = true;
1874  }
1875 
1876  if (blockedz) old_iPz = iP;
1877 
1878  if (old_iPt != vpImagePoint(-1,-1) && blockedt)
1879  {
1880  double diffi = iP.get_i() - old_iPt.get_i();
1881  double diffj = iP.get_j() - old_iPt.get_j();
1882  mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
1883  changed = true;
1884  }
1885 
1886  if (blockedt) old_iPt = iP;
1887 
1888  return mov;
1889 }
1890 
1891 
1897 {
1898  double width = vpMath::minimum(I.getWidth(),I.getHeight());
1899  vpImagePoint iP;
1900  vpImagePoint trash;
1901  bool clicked = false;
1902  bool clickedUp = false;
1904 
1905  vpHomogeneousMatrix mov(0,0,0,0,0,0);
1906  changed = false;
1907 
1908  //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1909 
1910  if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
1911 
1912  if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
1913 
1914  if(clicked)
1915  {
1916  if (b == vpMouseButton::button1) blockedr = true;
1917  if (b == vpMouseButton::button2) blockedz = true;
1918  if (b == vpMouseButton::button3) blockedt = true;
1919  blocked = true;
1920  }
1921  if(clickedUp)
1922  {
1923  if (b == vpMouseButton::button1)
1924  {
1925  old_iPr = vpImagePoint(-1,-1);
1926  blockedr = false;
1927  }
1928  if (b == vpMouseButton::button2)
1929  {
1930  old_iPz = vpImagePoint(-1,-1);
1931  blockedz = false;
1932  }
1933  if (b == vpMouseButton::button3)
1934  {
1935  old_iPt = vpImagePoint(-1,-1);
1936  blockedt = false;
1937  }
1938  if (!(blockedr || blockedz || blockedt))
1939  {
1940  blocked = false;
1941  while (vpDisplay::getClick(I,trash,b,false)) {};
1942  }
1943  }
1944 
1946 
1947  //std::cout << "point : " << iP << std::endl;
1948 
1949  double anglei = 0;
1950  double anglej = 0;
1951 
1952  if (old_iPr != vpImagePoint(-1,-1) && blockedr)
1953  {
1954  double diffi = iP.get_i() - old_iPr.get_i();
1955  double diffj = iP.get_j() - old_iPr.get_j();
1956  //cout << "delta :" << diffj << endl;;
1957  anglei = diffi*360/width;
1958  anglej = diffj*360/width;
1959  mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
1960  changed = true;
1961  }
1962 
1963  if (blockedr) old_iPr = iP;
1964 
1965  if (old_iPz != vpImagePoint(-1,-1) && blockedz)
1966  {
1967  double diffi = iP.get_i() - old_iPz.get_i();
1968  mov.buildFrom(0,0,diffi*0.01,0,0,0);
1969  changed = true;
1970  }
1971 
1972  if (blockedz) old_iPz = iP;
1973 
1974  if (old_iPt != vpImagePoint(-1,-1) && blockedt)
1975  {
1976  double diffi = iP.get_i() - old_iPt.get_i();
1977  double diffj = iP.get_j() - old_iPt.get_j();
1978  mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
1979  changed = true;
1980  }
1981 
1982  if (blockedt) old_iPt = iP;
1983 
1984  return mov;
1985 }
1986 
1992  const vpHomogeneousMatrix &fMo_)
1993 {
1994  vpPoint point;
1995  point.setWorldCoordinates(0,0,0);
1996 
1997  point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
1998 
1999  vpImagePoint iP;
2000 
2002 
2003  return iP;
2004 }
2005 
2011  const vpHomogeneousMatrix &cMo_,
2012  const vpHomogeneousMatrix &fMo_)
2013 {
2014  vpPoint point;
2015  point.setWorldCoordinates(0,0,0);
2016 
2017  point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
2018 
2019  vpImagePoint iP;
2020 
2022 
2023  return iP;
2024 }
2025 
2031  const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
2032 {
2033  vpPoint point;
2034  point.setWorldCoordinates(0,0,0);
2035 
2036  point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
2037 
2038  vpImagePoint iP;
2039 
2041 
2042  return iP;
2043 }
2044 
2050  const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
2051 {
2052  vpPoint point;
2053  point.setWorldCoordinates(0,0,0);
2054 
2055  point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
2056 
2057  vpImagePoint iP;
2058 
2060 
2061  return iP;
2062 }
2063 
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
vpDisplay * display
Definition: vpImage.h:121
A cylinder of 80cm length and 10cm radius.
unsigned int nbElements(void)
return the number of element in the list
Definition: vpList.h:259
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:315
A tire represented by 2 circles with radius 10cm and 15cm.
double get_i() const
Definition: vpImagePoint.h:195
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:429
unsigned int getWidth() const
Definition: vpImage.h:161
Three parallel lines with equation y=-5, y=0, y=5.
A plane represented by a 56cm by 56cm plate with a grid of 49 squares inside.
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:395
Provide simple list management.
Definition: vpList.h:113
A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
A cylindrical tool is attached to the camera.
static std::string getenv(const char *env)
Definition: vpIoTools.cpp:204
virtual bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)=0
std::list< vpImageSimulator > objectImage
error that can be emited by ViSP classes.
Definition: vpException.h:76
std::list< vpImagePoint > cameraTrajectory
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix cdMo
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
A 40cm by 40cm plate with 4 points at coordinates (-0.05,0.05,0), (0.05,0.05,0), (0.05,-0.05,0), (-0.05,-0.05,0). Each point is represented by a circle with 2cm radius.
vpHomogeneousMatrix fMo
double get_j() const
Definition: vpImagePoint.h:206
std::list< vpHomogeneousMatrix > fMoList
void next(void)
position the current element on the next one
Definition: vpList.h:273
static const vpColor red
Definition: vpColor.h:167
Class that defines what is a point.
Definition: vpPoint.h:65
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
4 points at coordinates (-0.03,-0.03,0), (0.03,-0.03,0), (0.03,0.03,0), (0.03,0.03,0). Each point is represented by a circle with 1cm radius.
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
void setCameraPosition(const vpHomogeneousMatrix &cMt)
virtual bool getPointerPosition(vpImagePoint &ip)=0
The object displayed at the desired position is a circle.
vpHomogeneousMatrix f2Mf
A 40cm by 40cm plate with 4 points at coordinates (-0.025,-0.05,0), (-0.075,0.05,0), (0.075,0.05,0), (0.025,-0.05,0). Each point is represented by a circle with 2cm radius.
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:384
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
type & value(void)
return the value of the current element
Definition: vpList.h:301
Class which enables to project an image in the 3D space and get the view of a virtual camera...
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
vpHomogeneousMatrix cMo
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1457
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:148
A 40cm by 40cm plate with 3 points at coordinates (0,0,0), (0.1,0,0), (0,0.1,0). Each point is repres...
void getExternalImage(vpImage< unsigned char > &I)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
static double rad(double deg)
Definition: vpMath.h:100
void displayTrajectory(const vpImage< unsigned char > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
vpCameraTrajectoryDisplayType camTrajType
A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0...
vpSceneDesiredObject desiredObject
A 40cm by 40cm plate with 4 points at coordinates (0,-0.1,0), (0.1,0,0), (0,0.1,0), (-0.1,0,0). Each point is represented by a circle with 2cm radius.
void getInternalImage(vpImage< unsigned char > &I)
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
Three parallel lines representing a road.
vpHomogeneousMatrix fMc
vpImagePoint projectCameraTrajectory(const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix rotz
A pipe represented by a cylinder of 25 cm length and 15cm radius.
unsigned int getHeight() const
Definition: vpImage.h:152
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:93
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
virtual void displayPoint(const vpImagePoint &ip, const vpColor &color)=0
std::list< vpHomogeneousMatrix > poseList
vpHomogeneousMatrix camMf2
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