Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Wire frame simulator
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
43 #include <visp3/robot/vpWireFrameSimulator.h>
44 #include <fcntl.h>
45 #include <string.h>
46 #include <stdio.h>
47 #include <vector>
48 
49 #include "vpClipping.h"
50 #include "vpToken.h"
51 #include "vpMy.h"
52 #include "vpLex.h"
53 #include "vpBound.h"
54 #include "vpKeyword.h"
55 #include "vpParser.h"
56 #include "vpArit.h"
57 #include "vpView.h"
58 #include "vpRfstack.h"
59 #include "vpVwstack.h"
60 #include "vpTmstack.h"
61 #include "vpCoreDisplay.h"
62 #include "vpBound.h"
63 #include "vpProjection.h"
64 #include "vpScene.h"
65 
66 #include <visp3/core/vpException.h>
67 #include <visp3/core/vpPoint.h>
68 #include <visp3/core/vpCameraParameters.h>
69 #include <visp3/core/vpMeterPixelConversion.h>
70 #include <visp3/core/vpPoint.h>
71 #include <visp3/core/vpIoTools.h>
72 
73 extern Point2i *point2i;
74 extern Point2i *listpoint2i;
75 
76 
77 
78 /*
79  Copy the scene corresponding to the registeresd parameters in the image.
80 */
81 void
82 vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
83 {
84  // extern Bound *clipping_Bound ();
85  Bound *bp, *bend;
86  Bound *clip; /* surface apres clipping */
87  Byte b = (Byte) *get_rfstack ();
88  Matrix m;
89 
90  //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
91  memmove((char *) m, (char *) mat, sizeof (Matrix));
92  View_to_Matrix (get_vwstack (), *(get_tmstack ()));
93  postmult_matrix (m, *(get_tmstack ()));
94  bp = sc.bound.ptr;
95  bend = bp + sc.bound.nbr;
96  for (; bp < bend; bp++)
97  {
98  if ((clip = clipping_Bound (bp, m)) != NULL)
99  {
100  Face *fp = clip->face.ptr;
101  Face *fend = fp + clip->face.nbr;
102 
103  set_Bound_face_display (clip, b); //regarde si is_visible
104 
105  point_3D_2D (clip->point.ptr, clip->point.nbr,(int)I.getWidth(),(int)I.getHeight(),point2i);
106  for (; fp < fend; fp++)
107  {
108  if (fp->is_visible)
109  {
110  wireframe_Face (fp, point2i);
111  Point2i *pt = listpoint2i;
112  for (int i = 1; i < fp->vertex.nbr; i++)
113  {
114  vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
115  pt++;
116  }
117  if (fp->vertex.nbr > 2)
118  {
119  vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
120  }
121  }
122  }
123  }
124  }
125 }
126 
127 /*
128  Copy the scene corresponding to the registeresd parameters in the image.
129 */
130 void
131 vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color)
132 {
133  //extern Bound *clipping_Bound ();
134 
135  Bound *bp, *bend;
136  Bound *clip; /* surface apres clipping */
137  Byte b = (Byte) *get_rfstack ();
138  Matrix m;
139 
140  //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
141  memmove((char *) m, (char *) mat, sizeof (Matrix));
142  View_to_Matrix (get_vwstack (), *(get_tmstack ()));
143  postmult_matrix (m, *(get_tmstack ()));
144  bp = sc.bound.ptr;
145  bend = bp + sc.bound.nbr;
146  for (; bp < bend; bp++)
147  {
148  if ((clip = clipping_Bound (bp, m)) != NULL)
149  {
150  Face *fp = clip->face.ptr;
151  Face *fend = fp + clip->face.nbr;
152 
153  set_Bound_face_display (clip, b); //regarde si is_visible
154 
155  point_3D_2D (clip->point.ptr, clip->point.nbr,(int)I.getWidth(),(int)I.getHeight(),point2i);
156  for (; fp < fend; fp++)
157  {
158  if (fp->is_visible)
159  {
160  wireframe_Face (fp, point2i);
161  Point2i *pt = listpoint2i;
162  for (int i = 1; i < fp->vertex.nbr; i++)
163  {
164  vpDisplay::displayLine(I,vpImagePoint((pt)->y,(pt)->x),vpImagePoint((pt+1)->y,(pt+1)->x),color,thickness_);
165  pt++;
166  }
167  if (fp->vertex.nbr > 2)
168  {
169  vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
170  }
171  }
172  }
173  }
174  }
175 }
176 
177 /*************************************************************************************************************/
178 
187 vpWireFrameSimulator::vpWireFrameSimulator()
188  : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(),
189  refMo(), cMo(), cdMo(), object(PLATE), desiredObject(D_STANDARD),
190  camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
191  desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true),
192  cameraTrajectory(), poseList(), fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(),
193  old_iPt(), blockedr(false), blockedz(false), blockedt(false), blocked(false),
194  camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
195  displayDesiredObject(false), displayCamera(false), displayImageSimulator(false),
196  cameraFactor(1.), camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
197 {
198  // set scene_dir from #define VISP_SCENE_DIR if it exists
199  // VISP_SCENES_DIR may contain multiple locations separated by ";"
200  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
201  bool sceneDirExists = false;
202  for(size_t i=0; i < scene_dirs.size(); i++)
203  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
204  scene_dir = scene_dirs[i];
205  sceneDirExists = true;
206  break;
207  }
208  if (! sceneDirExists) {
209  try {
210  scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
211  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
212  }
213  catch (...) {
214  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
215  }
216  }
217 
218  open_display();
219  open_clipping();
220 
221  old_iPr = vpImagePoint(-1,-1);
222  old_iPz = vpImagePoint(-1,-1);
223  old_iPt = vpImagePoint(-1,-1);
224 
225  rotz.buildFrom(0,0,0,0,0,vpMath::rad(180));
226 
227  scene.name = NULL;
228  scene.bound.ptr = NULL;
229  scene.bound.nbr = 0;
230 
231  desiredScene.name = NULL;
232  desiredScene.bound.ptr = NULL;
233  desiredScene.bound.nbr = 0;
234 
235  camera.name = NULL;
236  camera.bound.ptr = NULL;
237  camera.bound.nbr = 0;
238 }
239 
240 
244 vpWireFrameSimulator::~vpWireFrameSimulator()
245 {
246  if(sceneInitialized)
247  {
248  if(displayObject)
249  free_Bound_scene (&(this->scene));
250  if(displayCamera){
251  free_Bound_scene (&(this->camera));
252  }
253  if(displayDesiredObject)
254  free_Bound_scene (&(this->desiredScene));
255  }
256  close_clipping();
257  close_display ();
258 
259  cameraTrajectory.clear();
260  poseList.clear();
261  fMoList.clear();
262 }
263 
264 
274 void
275 vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
276 {
277  char name_cam[FILENAME_MAX];
278  char name[FILENAME_MAX];
279 
280  object = obj;
281  this->desiredObject = desired_object;
282 
283  const char *scene_dir_ = scene_dir.c_str();
284  if (strlen(scene_dir_) >= FILENAME_MAX) {
285  throw(vpException(vpException::memoryAllocationError,
286  "Not enough memory to intialize the camera name"));
287  }
288 
289  strcpy(name_cam, scene_dir_);
290  if (desiredObject != D_TOOL)
291  {
292  strcat(name_cam,"/camera.bnd");
293  set_scene(name_cam,&camera,cameraFactor);
294  }
295  else
296  {
297  strcat(name_cam,"/tool.bnd");
298  set_scene(name_cam,&(this->camera),1.0);
299  }
300 
301  strcpy(name, scene_dir_);
302  switch (obj)
303  {
304  case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
305  case CUBE : { strcat(name, "/cube.bnd"); break; }
306  case PLATE : { strcat(name, "/plate.bnd"); break; }
307  case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
308  case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
309  case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
310  case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
311  case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
312  case THREE_LINES : { strcat(name, "/line.bnd"); break; }
313  case ROAD : { strcat(name, "/road.bnd"); break; }
314  case TIRE : { strcat(name, "/circles2.bnd"); break; }
315  case PIPE : { strcat(name, "/pipe.bnd"); break; }
316  case CIRCLE : { strcat(name, "/circle.bnd"); break; }
317  case SPHERE : { strcat(name, "/sphere.bnd"); break; }
318  case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
319  case PLAN: { strcat(name, "/plan.bnd"); break; }
320  case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
321  }
322  set_scene(name,&(this->scene),1.0);
323 
324  scene_dir_ = scene_dir.c_str();
325  if (strlen(scene_dir_) >= FILENAME_MAX) {
326  throw(vpException(vpException::memoryAllocationError,
327  "Not enough memory to intialize the desired object name"));
328  }
329 
330  switch (desiredObject)
331  {
332  case D_STANDARD : { break; }
333  case D_CIRCLE : {
334  strcpy(name, scene_dir_);
335  strcat(name, "/circle_sq2.bnd");
336  break; }
337  case D_TOOL : {
338  strcpy(name, scene_dir_);
339  strcat(name, "/tool.bnd");
340  break; }
341  }
342  set_scene(name, &(this->desiredScene), 1.0);
343 
344  if (obj == PIPE) load_rfstack(IS_INSIDE);
345  else add_rfstack(IS_BACK);
346 
347  add_vwstack ("start","depth", 0.0, 100.0);
348  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
349  add_vwstack ("start","type", PERSPECTIVE);
350 
351  sceneInitialized = true;
352  displayObject = true;
353  displayDesiredObject = true;
354  displayCamera = true;
355  displayImageSimulator = true;
356 }
357 
370 void
371 vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object, const std::list<vpImageSimulator> &imObj)
372 {
373  initScene(obj, desired_object);
374  objectImage = imObj;
375  displayImageSimulator = true;
376 }
377 
378 
388 void
389 vpWireFrameSimulator::initScene(const char* obj, const char* desired_object)
390 {
391  char name_cam[FILENAME_MAX];
392  char name[FILENAME_MAX];
393 
394  object = THREE_PTS;
395  this->desiredObject = D_STANDARD;
396 
397  const char *scene_dir_ = scene_dir.c_str();
398  if (strlen(scene_dir_) >= FILENAME_MAX) {
399  throw(vpException(vpException::memoryAllocationError,
400  "Not enough memory to intialize the camera name"));
401  }
402 
403  strcpy(name_cam, scene_dir_);
404  strcat(name_cam,"/camera.bnd");
405  set_scene(name_cam,&camera,cameraFactor);
406 
407  if (strlen(obj) >= FILENAME_MAX) {
408  throw(vpException(vpException::memoryAllocationError,
409  "Not enough memory to intialize the name"));
410  }
411 
412  strcpy(name,obj);
413  Model_3D model;
414  model = getExtension(obj);
415  if (model == BND_MODEL)
416  set_scene(name,&(this->scene),1.0);
417  else if (model == WRL_MODEL)
418  set_scene_wrl(name,&(this->scene),1.0);
419  else if (model == UNKNOWN_MODEL)
420  {
421  vpERROR_TRACE("Unknown file extension for the 3D model");
422  }
423 
424  if (strlen(desired_object) >= FILENAME_MAX) {
425  throw(vpException(vpException::memoryAllocationError,
426  "Not enough memory to intialize the camera name"));
427  }
428 
429  strcpy(name,desired_object);
430  model = getExtension(desired_object);
431  if (model == BND_MODEL)
432  set_scene(name,&(this->desiredScene),1.0);
433  else if (model == WRL_MODEL)
434  set_scene_wrl(name,&(this->desiredScene),1.0);
435  else if (model == UNKNOWN_MODEL)
436  {
437  vpERROR_TRACE("Unknown file extension for the 3D model");
438  }
439 
440  add_rfstack(IS_BACK);
441 
442  add_vwstack ("start","depth", 0.0, 100.0);
443  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
444  add_vwstack ("start","type", PERSPECTIVE);
445 
446  sceneInitialized = true;
447  displayObject = true;
448  displayDesiredObject = true;
449  displayCamera = true;
450 }
451 
464 void
465 vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, const std::list<vpImageSimulator> &imObj)
466 {
467  initScene(obj, desired_object);
468  objectImage = imObj;
469  displayImageSimulator = true;
470 }
471 
480 void
481 vpWireFrameSimulator::initScene(const vpSceneObject &obj)
482 {
483  char name_cam[FILENAME_MAX];
484  char name[FILENAME_MAX];
485 
486  object = obj;
487 
488  const char *scene_dir_ = scene_dir.c_str();
489  if (strlen(scene_dir_) >= FILENAME_MAX) {
490  throw(vpException(vpException::memoryAllocationError,
491  "Not enough memory to intialize the camera name"));
492  }
493 
494  strcpy(name_cam, scene_dir_);
495  strcat(name_cam,"/camera.bnd");
496  set_scene(name_cam,&camera,cameraFactor);
497 
498  strcpy(name, scene_dir_);
499  switch (obj)
500  {
501  case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
502  case CUBE : { strcat(name, "/cube.bnd"); break; }
503  case PLATE : { strcat(name, "/plate.bnd"); break; }
504  case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
505  case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
506  case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
507  case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
508  case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
509  case THREE_LINES : { strcat(name, "/line.bnd"); break; }
510  case ROAD : { strcat(name, "/road.bnd"); break; }
511  case TIRE : { strcat(name, "/circles2.bnd"); break; }
512  case PIPE : { strcat(name, "/pipe.bnd"); break; }
513  case CIRCLE : { strcat(name, "/circle.bnd"); break; }
514  case SPHERE : { strcat(name, "/sphere.bnd"); break; }
515  case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
516  case PLAN: { strcat(name, "/plan.bnd"); break; }
517  case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
518  }
519  set_scene(name,&(this->scene),1.0);
520 
521  if (obj == PIPE) load_rfstack(IS_INSIDE);
522  else add_rfstack(IS_BACK);
523 
524  add_vwstack ("start","depth", 0.0, 100.0);
525  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
526  add_vwstack ("start","type", PERSPECTIVE);
527 
528  sceneInitialized = true;
529  displayObject = true;
530  displayCamera = true;
531 
532  displayDesiredObject = false;
533  displayImageSimulator = false;
534 }
535 
547 void
548 vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
549 {
550  initScene(obj);
551  objectImage = imObj;
552  displayImageSimulator = true;
553 }
554 
563 void
564 vpWireFrameSimulator::initScene(const char* obj)
565 {
566  char name_cam[FILENAME_MAX];
567  char name[FILENAME_MAX];
568 
569  object = THREE_PTS;
570 
571  const char *scene_dir_ = scene_dir.c_str();
572  if (strlen(scene_dir_) >= FILENAME_MAX) {
573  throw(vpException(vpException::memoryAllocationError,
574  "Not enough memory to intialize the camera name"));
575  }
576 
577  strcpy(name_cam, scene_dir_);
578  strcat(name_cam,"/camera.bnd");
579  set_scene(name_cam,&camera,cameraFactor);
580 
581  if (strlen(obj) >= FILENAME_MAX) {
582  throw(vpException(vpException::memoryAllocationError,
583  "Not enough memory to intialize the name"));
584  }
585 
586  strcpy(name,obj);
587  Model_3D model;
588  model = getExtension(obj);
589  if (model == BND_MODEL)
590  set_scene(name,&(this->scene),1.0);
591  else if (model == WRL_MODEL)
592  set_scene_wrl(name,&(this->scene),1.0);
593  else if (model == UNKNOWN_MODEL)
594  {
595  vpERROR_TRACE("Unknown file extension for the 3D model");
596  }
597 
598  add_rfstack(IS_BACK);
599 
600  add_vwstack ("start","depth", 0.0, 100.0);
601  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
602  add_vwstack ("start","type", PERSPECTIVE);
603 
604  sceneInitialized = true;
605  displayObject = true;
606  displayCamera = true;
607 }
608 
620 void
621 vpWireFrameSimulator::initScene(const char* obj, const std::list<vpImageSimulator> &imObj)
622 {
623  initScene(obj);
624  objectImage = imObj;
625  displayImageSimulator = true;
626 }
627 
635 void
636 vpWireFrameSimulator::getInternalImage(vpImage<vpRGBa> &I)
637 {
638  if (!sceneInitialized)
639  throw(vpException(vpException::notInitialized,"The scene has to be initialized")) ;
640 
641  double u;
642  double v;
643  //if(px_int != 1 && py_int != 1)
644  // we assume px_int and py_int > 0
645  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
646  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
647  {
648  u = (double)I.getWidth()/(2*px_int);
649  v = (double)I.getHeight()/(2*py_int);
650  }
651  else
652  {
653  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
654  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
655  }
656 
657  float o44c[4][4],o44cd[4][4],x,y,z;
658  Matrix id = IDENTITY_MATRIX;
659 
660  vp2jlc_matrix(cMo.inverse(),o44c);
661  vp2jlc_matrix(cdMo.inverse(),o44cd);
662 
663  if (displayImageSimulator)
664  {
665  I = 255;
666 
667  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
668  vpImageSimulator* imSim = &(*it);
669  imSim->setCameraPosition(rotz*cMo);
670  imSim->getImage(I,getInternalCameraParameters(I));
671  }
672 
673  if (I.display != NULL)
674  vpDisplay::display(I);
675  }
676 
677  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
678  x = o44c[2][0] + o44c[3][0];
679  y = o44c[2][1] + o44c[3][1];
680  z = o44c[2][2] + o44c[3][2];
681  add_vwstack ("start","vrp", x,y,z);
682  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
683  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
684  add_vwstack ("start","window", -u, u, -v, v);
685  if (displayObject)
686  display_scene(id,this->scene,I, curColor);
687 
688 
689  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
690  x = o44cd[2][0] + o44cd[3][0];
691  y = o44cd[2][1] + o44cd[3][1];
692  z = o44cd[2][2] + o44cd[3][2];
693  add_vwstack ("start","vrp", x,y,z);
694  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
695  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
696  add_vwstack ("start","window", -u, u, -v, v);
697  if (displayDesiredObject)
698  {
699  if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
700  else display_scene(id,desiredScene,I, desColor);
701  }
702 }
703 
704 
713 void
714 vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I)
715 {
716  bool changed = false;
717  vpHomogeneousMatrix displacement = navigation(I,changed);
718 
719  //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
720  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
721  camMf2 = camMf2*displacement;
722 
723  f2Mf = camMf2.inverse()*camMf;
724 
725  camMf = camMf2* displacement * f2Mf;
726 
727  double u;
728  double v;
729  //if(px_ext != 1 && py_ext != 1)
730  // we assume px_ext and py_ext > 0
731  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
732  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
733  {
734  u = (double)I.getWidth()/(2*px_ext);
735  v = (double)I.getHeight()/(2*py_ext);
736  }
737  else
738  {
739  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
740  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
741  }
742 
743  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
744 
745  vp2jlc_matrix(camMf.inverse(),w44cext);
746  vp2jlc_matrix(fMc,w44c);
747  vp2jlc_matrix(fMo,w44o);
748 
749 
750  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
751  x = w44cext[2][0] + w44cext[3][0];
752  y = w44cext[2][1] + w44cext[3][1];
753  z = w44cext[2][2] + w44cext[3][2];
754  add_vwstack ("start","vrp", x,y,z);
755  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
756  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
757  add_vwstack ("start","window", -u, u, -v, v);
758  if ((object == CUBE) || (object == SPHERE))
759  {
760  add_vwstack ("start","type", PERSPECTIVE);
761  }
762 
763  if (displayImageSimulator)
764  {
765  I = 255;
766 
767  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
768  vpImageSimulator* imSim = &(*it);
769  imSim->setCameraPosition(rotz*camMf*fMo);
770  imSim->getImage(I,getInternalCameraParameters(I));
771  }
772 
773  if (I.display != NULL)
774  vpDisplay::display(I);
775  }
776 
777  if (displayObject)
778  display_scene(w44o,this->scene,I, curColor);
779 
780  if (displayCamera)
781  display_scene(w44c,camera, I, camColor);
782 
783  if (displayCameraTrajectory)
784  {
785  vpImagePoint iP;
786  vpImagePoint iP_1;
787  poseList.push_back(cMo);
788  fMoList.push_back(fMo);
789 
790  int iter = 0;
791 
792  if (changed || extCamChanged)
793  {
794  cameraTrajectory.clear();
795  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
796  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
797 
798  while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end()))
799  {
800  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
801  cameraTrajectory.push_back(iP);
802  if (camTrajType == CT_LINE)
803  {
804  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
805  }
806  else if (camTrajType == CT_POINT)
807  vpDisplay::displayPoint(I,iP,camTrajColor);
808  ++iter_poseList;
809  ++iter_fMoList;
810  iter++;
811  iP_1 = iP;
812  }
813  extCamChanged = false;
814  }
815  else
816  {
817  iP = projectCameraTrajectory(I, cMo, fMo);
818  cameraTrajectory.push_back(iP);
819 
820  for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
821  if (camTrajType == CT_LINE)
822  {
823  if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
824  }
825  else if (camTrajType == CT_POINT)
826  vpDisplay::displayPoint(I, *it, camTrajColor);
827  iter++;
828  iP_1 = *it;
829  }
830  }
831 
832  if (poseList.size() > nbrPtLimit)
833  {
834  poseList.pop_front();
835  }
836  if (fMoList.size() > nbrPtLimit)
837  {
838  fMoList.pop_front();
839  }
840  if (cameraTrajectory.size() > nbrPtLimit)
841  {
842  cameraTrajectory.pop_front();
843  }
844  }
845 }
846 
847 
856 void
857 vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cam_Mf)
858 {
859  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
860 
861  vpHomogeneousMatrix camMft = rotz * cam_Mf;
862 
863  double u;
864  double v;
865  //if(px_ext != 1 && py_ext != 1)
866  // we assume px_ext and py_ext > 0
867  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
868  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
869  {
870  u = (double)I.getWidth()/(2*px_ext);
871  v = (double)I.getHeight()/(2*py_ext);
872  }
873  else
874  {
875  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
876  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
877  }
878 
879  vp2jlc_matrix(camMft.inverse(),w44cext);
880  vp2jlc_matrix(fMo*cMo.inverse(),w44c);
881  vp2jlc_matrix(fMo,w44o);
882 
883  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
884  x = w44cext[2][0] + w44cext[3][0];
885  y = w44cext[2][1] + w44cext[3][1];
886  z = w44cext[2][2] + w44cext[3][2];
887  add_vwstack ("start","vrp", x,y,z);
888  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
889  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
890  add_vwstack ("start","window", -u, u, -v, v);
891 
892  if (displayImageSimulator)
893  {
894  I = 255;
895 
896  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
897  vpImageSimulator* imSim = &(*it);
898  imSim->setCameraPosition(rotz*cam_Mf*fMo);
899  imSim->getImage(I,getInternalCameraParameters(I));
900  }
901 
902  if (I.display != NULL)
903  vpDisplay::display(I);
904  }
905 
906  if (displayObject)
907  display_scene(w44o,this->scene,I, curColor);
908  if (displayCamera)
909  display_scene(w44c,camera, I, camColor);
910 }
911 
912 
920 void
921 vpWireFrameSimulator::getInternalImage(vpImage<unsigned char> &I)
922 {
923  if (!sceneInitialized)
924  throw(vpException(vpException::notInitialized,"The scene has to be initialized")) ;
925 
926  double u;
927  double v;
928  //if(px_int != 1 && py_int != 1)
929  // we assume px_int and py_int > 0
930  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
931  && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
932  {
933  u = (double)I.getWidth()/(2*px_int);
934  v = (double)I.getHeight()/(2*py_int);
935  }
936  else
937  {
938  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
939  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
940  }
941 
942  float o44c[4][4],o44cd[4][4],x,y,z;
943  Matrix id = IDENTITY_MATRIX;
944 
945  vp2jlc_matrix(cMo.inverse(),o44c);
946  vp2jlc_matrix(cdMo.inverse(),o44cd);
947 
948  if (displayImageSimulator)
949  {
950  I = 255;
951 
952  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
953  vpImageSimulator* imSim = &(*it);
954  imSim->setCameraPosition(rotz*camMf*fMo);
955  imSim->getImage(I,getInternalCameraParameters(I));
956  }
957 
958  if (I.display != NULL)
959  vpDisplay::display(I);
960  }
961 
962  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
963  x = o44c[2][0] + o44c[3][0];
964  y = o44c[2][1] + o44c[3][1];
965  z = o44c[2][2] + o44c[3][2];
966  add_vwstack ("start","vrp", x,y,z);
967  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
968  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
969  add_vwstack ("start","window", -u, u, -v, v);
970  if (displayObject)
971  display_scene(id,this->scene,I, curColor);
972 
973 
974  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
975  x = o44cd[2][0] + o44cd[3][0];
976  y = o44cd[2][1] + o44cd[3][1];
977  z = o44cd[2][2] + o44cd[3][2];
978  add_vwstack ("start","vrp", x,y,z);
979  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
980  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
981  add_vwstack ("start","window", -u, u, -v, v);
982  if (displayDesiredObject)
983  {
984  if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
985  else display_scene(id,desiredScene,I, desColor);
986  }
987 }
988 
989 
998 void
999 vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I)
1000 {
1001  bool changed = false;
1002  vpHomogeneousMatrix displacement = navigation(I,changed);
1003 
1004  //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1005  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1006  camMf2 = camMf2*displacement;
1007 
1008  f2Mf = camMf2.inverse()*camMf;
1009 
1010  camMf = camMf2* displacement * f2Mf;
1011 
1012  double u;
1013  double v;
1014  //if(px_ext != 1 && py_ext != 1)
1015  // we assume px_ext and py_ext > 0
1016  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
1017  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
1018  {
1019  u = (double)I.getWidth()/(2*px_ext);
1020  v = (double)I.getHeight()/(2*py_ext);
1021  }
1022  else
1023  {
1024  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1025  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1026  }
1027 
1028  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
1029 
1030  vp2jlc_matrix(camMf.inverse(),w44cext);
1031  vp2jlc_matrix(fMc,w44c);
1032  vp2jlc_matrix(fMo,w44o);
1033 
1034 
1035  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
1036  x = w44cext[2][0] + w44cext[3][0];
1037  y = w44cext[2][1] + w44cext[3][1];
1038  z = w44cext[2][2] + w44cext[3][2];
1039  add_vwstack ("start","vrp", x,y,z);
1040  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
1041  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
1042  add_vwstack ("start","window", -u, u, -v, v);
1043  if ((object == CUBE) || (object == SPHERE))
1044  {
1045  add_vwstack ("start","type", PERSPECTIVE);
1046  }
1047 
1048  if (displayImageSimulator)
1049  {
1050  I = 255;
1051  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1052  vpImageSimulator* imSim = &(*it);
1053  imSim->setCameraPosition(rotz*camMf*fMo);
1054  imSim->getImage(I,getInternalCameraParameters(I));
1055  }
1056  if (I.display != NULL)
1057  vpDisplay::display(I);
1058  }
1059 
1060  if (displayObject)
1061  display_scene(w44o,this->scene,I, curColor);
1062 
1063  if (displayCamera)
1064  display_scene(w44c,camera, I, camColor);
1065 
1066  if (displayCameraTrajectory)
1067  {
1068  vpImagePoint iP;
1069  vpImagePoint iP_1;
1070  poseList.push_back(cMo);
1071  fMoList.push_back(fMo);
1072 
1073  int iter = 0;
1074 
1075  if (changed || extCamChanged)
1076  {
1077  cameraTrajectory.clear();
1078  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
1079  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
1080 
1081  while ((iter_poseList!=poseList.end()) && (iter_fMoList!=fMoList.end()) )
1082  {
1083  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
1084  cameraTrajectory.push_back(iP);
1085  //vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
1086  if (camTrajType == CT_LINE)
1087  {
1088  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
1089  }
1090  else if (camTrajType == CT_POINT)
1091  vpDisplay::displayPoint(I,iP,camTrajColor);
1092  ++iter_poseList;
1093  ++iter_fMoList;
1094  iter++;
1095  iP_1 = iP;
1096  }
1097  extCamChanged = false;
1098  }
1099  else
1100  {
1101  iP = projectCameraTrajectory(I, cMo,fMo);
1102  cameraTrajectory.push_back(iP);
1103 
1104  for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
1105  if (camTrajType == CT_LINE){
1106  if (iter != 0) vpDisplay::displayLine(I,iP_1,*it,camTrajColor, thickness_);
1107  }
1108  else if(camTrajType == CT_POINT)
1109  vpDisplay::displayPoint(I, *it, camTrajColor);
1110  iter++;
1111  iP_1 = *it;
1112  }
1113  }
1114 
1115  if (poseList.size() > nbrPtLimit)
1116  {
1117  poseList.pop_front();
1118  }
1119  if (fMoList.size() > nbrPtLimit)
1120  {
1121  fMoList.pop_front();
1122  }
1123  if (cameraTrajectory.size() > nbrPtLimit)
1124  {
1125  cameraTrajectory.pop_front();
1126  }
1127  }
1128 }
1129 
1130 
1139 void
1140 vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cam_Mf)
1141 {
1142  float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
1143 
1144  vpHomogeneousMatrix camMft = rotz * cam_Mf;
1145 
1146  double u;
1147  double v;
1148  //if(px_ext != 1 && py_ext != 1)
1149  // we assume px_ext and py_ext > 0
1150  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
1151  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
1152  {
1153  u = (double)I.getWidth()/(2*px_ext);
1154  v = (double)I.getHeight()/(2*py_ext);
1155  }
1156  else
1157  {
1158  u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1159  v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
1160  }
1161 
1162  vp2jlc_matrix(camMft.inverse(),w44cext);
1163  vp2jlc_matrix(fMo*cMo.inverse(),w44c);
1164  vp2jlc_matrix(fMo,w44o);
1165 
1166  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
1167  x = w44cext[2][0] + w44cext[3][0];
1168  y = w44cext[2][1] + w44cext[3][1];
1169  z = w44cext[2][2] + w44cext[3][2];
1170  add_vwstack ("start","vrp", x,y,z);
1171  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
1172  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
1173  add_vwstack ("start","window", -u, u, -v, v);
1174 
1175  if (displayImageSimulator)
1176  {
1177  I = 255;
1178  for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
1179  vpImageSimulator* imSim = &(*it);
1180  imSim->setCameraPosition(rotz*cam_Mf*fMo);
1181  imSim->getImage(I,getInternalCameraParameters(I));
1182  }
1183  if (I.display != NULL)
1184  vpDisplay::display(I);
1185  }
1186 
1187  if (displayObject)
1188  display_scene(w44o,this->scene,I, curColor);
1189  if (displayCamera)
1190  display_scene(w44c,camera, I, camColor);
1191 }
1192 
1203 void
1204 vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
1205  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
1206 {
1207  if (list_cMo.size() != list_fMo.size())
1208  throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
1209 
1210  vpImagePoint iP;
1211  vpImagePoint iP_1;
1212  int iter = 0;
1213 
1214  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1215  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1216 
1217  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
1218  {
1219  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1220  if (camTrajType == CT_LINE)
1221  {
1222  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
1223  }
1224  else if (camTrajType == CT_POINT)
1225  vpDisplay::displayPoint(I,iP,camTrajColor);
1226  ++it_cMo;
1227  ++it_fMo;
1228  iter++;
1229  iP_1 = iP;
1230  }
1231 }
1232 
1243 void
1244 vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
1245  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
1246 {
1247  if (list_cMo.size() != list_fMo.size())
1248  throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
1249 
1250  vpImagePoint iP;
1251  vpImagePoint iP_1;
1252  int iter = 0;
1253 
1254  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1255  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1256 
1257  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
1258  {
1259  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1260  if (camTrajType == CT_LINE)
1261  {
1262  if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
1263  }
1264  else if (camTrajType == CT_POINT)
1265  vpDisplay::displayPoint(I,iP,camTrajColor);
1266  ++it_cMo;
1267  ++it_fMo;
1268  iter++;
1269  iP_1 = iP;
1270  }
1271 }
1272 
1273 
1277 vpHomogeneousMatrix
1278 vpWireFrameSimulator::navigation(const vpImage<vpRGBa> &I, bool &changed)
1279 {
1280  double width = vpMath::minimum(I.getWidth(),I.getHeight());
1281  vpImagePoint iP;
1282  vpImagePoint trash;
1283  bool clicked = false;
1284  bool clickedUp = false;
1285  vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
1286 
1287  vpHomogeneousMatrix mov(0,0,0,0,0,0);
1288  changed = false;
1289 
1290  //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1291 
1292  if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
1293 
1294  if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
1295 
1296  if(clicked)
1297  {
1298  if (b == vpMouseButton::button1) blockedr = true;
1299  if (b == vpMouseButton::button2) blockedz = true;
1300  if (b == vpMouseButton::button3) blockedt = true;
1301  blocked = true;
1302  }
1303  if(clickedUp)
1304  {
1305  if (b == vpMouseButton::button1)
1306  {
1307  old_iPr = vpImagePoint(-1,-1);
1308  blockedr = false;
1309  }
1310  if (b == vpMouseButton::button2)
1311  {
1312  old_iPz = vpImagePoint(-1,-1);
1313  blockedz = false;
1314  }
1315  if (b == vpMouseButton::button3)
1316  {
1317  old_iPt = vpImagePoint(-1,-1);
1318  blockedt = false;
1319  }
1320  if (!(blockedr || blockedz || blockedt))
1321  {
1322  blocked = false;
1323  while (vpDisplay::getClick(I,trash,b,false)) {};
1324  }
1325  }
1326 
1327  vpDisplay::getPointerPosition(I,iP);
1328 
1329  if (old_iPr != vpImagePoint(-1,-1) && blockedr)
1330  {
1331  double diffi = iP.get_i() - old_iPr.get_i();
1332  double diffj = iP.get_j() - old_iPr.get_j();
1333  //cout << "delta :" << diffj << endl;;
1334  double anglei = diffi*360/width;
1335  double anglej = diffj*360/width;
1336  mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
1337  changed = true;
1338  }
1339 
1340  if (blockedr) old_iPr = iP;
1341 
1342  if (old_iPz != vpImagePoint(-1,-1) && blockedz)
1343  {
1344  double diffi = iP.get_i() - old_iPz.get_i();
1345  mov.buildFrom(0,0,diffi*0.01,0,0,0);
1346  changed = true;
1347  }
1348 
1349  if (blockedz) old_iPz = iP;
1350 
1351  if (old_iPt != vpImagePoint(-1,-1) && blockedt)
1352  {
1353  double diffi = iP.get_i() - old_iPt.get_i();
1354  double diffj = iP.get_j() - old_iPt.get_j();
1355  mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
1356  changed = true;
1357  }
1358 
1359  if (blockedt) old_iPt = iP;
1360 
1361  return mov;
1362 }
1363 
1364 
1368 vpHomogeneousMatrix
1369 vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
1370 {
1371  double width = vpMath::minimum(I.getWidth(),I.getHeight());
1372  vpImagePoint iP;
1373  vpImagePoint trash;
1374  bool clicked = false;
1375  bool clickedUp = false;
1376  vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
1377 
1378  vpHomogeneousMatrix mov(0,0,0,0,0,0);
1379  changed = false;
1380 
1381  //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1382 
1383  if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
1384 
1385  if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
1386 
1387  if(clicked)
1388  {
1389  if (b == vpMouseButton::button1) blockedr = true;
1390  if (b == vpMouseButton::button2) blockedz = true;
1391  if (b == vpMouseButton::button3) blockedt = true;
1392  blocked = true;
1393  }
1394  if(clickedUp)
1395  {
1396  if (b == vpMouseButton::button1)
1397  {
1398  old_iPr = vpImagePoint(-1,-1);
1399  blockedr = false;
1400  }
1401  if (b == vpMouseButton::button2)
1402  {
1403  old_iPz = vpImagePoint(-1,-1);
1404  blockedz = false;
1405  }
1406  if (b == vpMouseButton::button3)
1407  {
1408  old_iPt = vpImagePoint(-1,-1);
1409  blockedt = false;
1410  }
1411  if (!(blockedr || blockedz || blockedt))
1412  {
1413  blocked = false;
1414  while (vpDisplay::getClick(I,trash,b,false)) {};
1415  }
1416  }
1417 
1418  vpDisplay::getPointerPosition(I,iP);
1419 
1420  //std::cout << "point : " << iP << std::endl;
1421 
1422  if (old_iPr != vpImagePoint(-1,-1) && blockedr)
1423  {
1424  double diffi = iP.get_i() - old_iPr.get_i();
1425  double diffj = iP.get_j() - old_iPr.get_j();
1426  //cout << "delta :" << diffj << endl;;
1427  double anglei = diffi*360/width;
1428  double anglej = diffj*360/width;
1429  mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
1430  changed = true;
1431  }
1432 
1433  if (blockedr) old_iPr = iP;
1434 
1435  if (old_iPz != vpImagePoint(-1,-1) && blockedz)
1436  {
1437  double diffi = iP.get_i() - old_iPz.get_i();
1438  mov.buildFrom(0,0,diffi*0.01,0,0,0);
1439  changed = true;
1440  }
1441 
1442  if (blockedz) old_iPz = iP;
1443 
1444  if (old_iPt != vpImagePoint(-1,-1) && blockedt)
1445  {
1446  double diffi = iP.get_i() - old_iPt.get_i();
1447  double diffj = iP.get_j() - old_iPt.get_j();
1448  mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
1449  changed = true;
1450  }
1451 
1452  if (blockedt) old_iPt = iP;
1453 
1454  return mov;
1455 }
1456 
1460 vpImagePoint
1461 vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
1462  const vpHomogeneousMatrix &fMo_)
1463 {
1464  vpPoint point;
1465  point.setWorldCoordinates(0,0,0);
1466 
1467  point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
1468 
1469  vpImagePoint iP;
1470 
1471  vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
1472 
1473  return iP;
1474 }
1475 
1479 vpImagePoint
1480 vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I,
1481  const vpHomogeneousMatrix &cMo_,
1482  const vpHomogeneousMatrix &fMo_)
1483 {
1484  vpPoint point;
1485  point.setWorldCoordinates(0,0,0);
1486 
1487  point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
1488 
1489  vpImagePoint iP;
1490 
1491  vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
1492 
1493  return iP;
1494 }
1495 
1499 vpImagePoint
1500 vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
1501  const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
1502 {
1503  vpPoint point;
1504  point.setWorldCoordinates(0,0,0);
1505 
1506  point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
1507 
1508  vpImagePoint iP;
1509 
1510  vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
1511 
1512  return iP;
1513 }
1514 
1518 vpImagePoint
1519 vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
1520  const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
1521 {
1522  vpPoint point;
1523  point.setWorldCoordinates(0,0,0);
1524 
1525  point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
1526 
1527  vpImagePoint iP;
1528 
1529  vpMeterPixelConversion::convertPoint ( getExternalCameraParameters(I), point.get_x(), point.get_y(),iP );
1530 
1531  return iP;
1532 }
1533 
vpWireFrameSimulator::D_STANDARD
The object displayed at the desired position is the same than the scene object defined in vpSceneObje...
Definition: vpWireFrameSimulator.h:178
vpImage::display
vpDisplay * display
Definition: vpImage.h:135
vpWireFrameSimulator::CYLINDER
A cylinder of 80cm length and 10cm radius.
Definition: vpWireFrameSimulator.h:165
vpDisplay::getPointerPosition
static bool getPointerPosition(const vpImage< unsigned char > &I, vpImagePoint &ip)
Definition: vpDisplay_uchar.cpp:1083
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:657
vpIoTools::checkDirectory
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:358
vpWireFrameSimulator::TIRE
A tire represented by 2 circles with radius 10cm and 15cm.
Definition: vpWireFrameSimulator.h:161
vpImagePoint::get_i
double get_i() const
Definition: vpImagePoint.h:199
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:226
vpWireFrameSimulator::THREE_LINES
Three parallel lines with equation y=-5, y=0, y=5.
Definition: vpWireFrameSimulator.h:159
vpWireFrameSimulator::PLAN
A plane represented by a 56cm by 56cm plate with a grid of 49 squares inside.
Definition: vpWireFrameSimulator.h:166
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:93
vpWireFrameSimulator::POINT_CLOUD
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.
Definition: vpWireFrameSimulator.h:167
vpImageSimulator::getImage
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
Definition: vpImageSimulator.cpp:197
vpWireFrameSimulator::PLATE
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.
Definition: vpWireFrameSimulator.h:153
vpMeterPixelConversion::convertPoint
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 ...
Definition: vpMeterPixelConversion.h:102
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:391
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
vpWireFrameSimulator::D_TOOL
A cylindrical tool is attached to the camera.
Definition: vpWireFrameSimulator.h:180
vpIoTools::getenv
static std::string getenv(const char *env)
Definition: vpIoTools.cpp:244
vpDisplay::getClickUp
static bool getClickUp(const vpImage< unsigned char > &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)
Definition: vpDisplay_uchar.cpp:756
vpWireFrameSimulator::objectImage
std::list< vpImageSimulator > objectImage
Definition: vpWireFrameSimulator.h:193
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:73
vpWireFrameSimulator::cameraTrajectory
std::list< vpImagePoint > cameraTrajectory
Definition: vpWireFrameSimulator.h:213
vpDisplay::displayPoint
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:364
vpForwardProjection::track
void track(const vpHomogeneousMatrix &cMo)
Definition: vpForwardProjection.cpp:126
vpWireFrameSimulator::cdMo
vpHomogeneousMatrix cdMo
Definition: vpWireFrameSimulator.h:200
vpPoint::get_y
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
vpWireFrameSimulator::SQUARE_10CM
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.
Definition: vpWireFrameSimulator.h:156
vpWireFrameSimulator::fMo
vpHomogeneousMatrix fMo
Definition: vpWireFrameSimulator.h:195
vpImagePoint::get_j
double get_j() const
Definition: vpImagePoint.h:210
vpWireFrameSimulator::fMoList
std::list< vpHomogeneousMatrix > fMoList
Definition: vpWireFrameSimulator.h:215
vpColor::red
static const vpColor red
Definition: vpColor.h:163
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:59
vpMath::maximum
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:140
vpWireFrameSimulator::SMALL_PLATE
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.
Definition: vpWireFrameSimulator.h:154
vpWireFrameSimulator::getExternalCameraParameters
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
Definition: vpWireFrameSimulator.h:276
vpImageSimulator::setCameraPosition
void setCameraPosition(const vpHomogeneousMatrix &cMt)
Definition: vpImageSimulator.cpp:1041
vpWireFrameSimulator::D_CIRCLE
The object displayed at the desired position is a circle.
Definition: vpWireFrameSimulator.h:179
vpWireFrameSimulator::f2Mf
vpHomogeneousMatrix f2Mf
Definition: vpWireFrameSimulator.h:227
vpWireFrameSimulator::TRAPEZOID
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.
Definition: vpWireFrameSimulator.h:158
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:624
vpImageSimulator
Class which enables to project an image in the 3D space and get the view of a virtual camera...
Definition: vpImageSimulator.h:133
vpWireFrameSimulator::initScene
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
Definition: vpWireFrameSimulator.cpp:275
vpWireFrameSimulator::cMo
vpHomogeneousMatrix cMo
Definition: vpWireFrameSimulator.h:199
vpPoint::get_x
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
vpIoTools::splitChain
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1596
vpMath::minimum
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
vpWireFrameSimulator::THREE_PTS
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...
Definition: vpWireFrameSimulator.h:151
vpWireFrameSimulator::getExternalImage
void getExternalImage(vpImage< unsigned char > &I)
Definition: vpWireFrameSimulator.cpp:999
vpHomogeneousMatrix::buildFrom
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:237
vpWireFrameSimulator::getInternalCameraParameters
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
Definition: vpWireFrameSimulator.h:326
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:104
vpWireFrameSimulator::displayTrajectory
void displayTrajectory(const vpImage< unsigned char > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
Definition: vpWireFrameSimulator.cpp:1204
vpWireFrameSimulator::camTrajType
vpCameraTrajectoryDisplayType camTrajType
Definition: vpWireFrameSimulator.h:241
vpWireFrameSimulator::RECTANGLE
A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0...
Definition: vpWireFrameSimulator.h:155
vpWireFrameSimulator::desiredObject
vpSceneDesiredObject desiredObject
Definition: vpWireFrameSimulator.h:203
vpWireFrameSimulator::DIAMOND
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.
Definition: vpWireFrameSimulator.h:157
vpWireFrameSimulator::getInternalImage
void getInternalImage(vpImage< unsigned char > &I)
Definition: vpWireFrameSimulator.cpp:921
vpWireFrameSimulator::display_scene
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
Definition: vpWireFrameSimulator.cpp:82
vpWireFrameSimulator::camMf
vpHomogeneousMatrix camMf
Definition: vpWireFrameSimulator.h:197
vpWireFrameSimulator::navigation
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
Definition: vpWireFrameSimulator.cpp:1278
vpWireFrameSimulator::ROAD
Three parallel lines representing a road.
Definition: vpWireFrameSimulator.h:160
vpWireFrameSimulator::fMc
vpHomogeneousMatrix fMc
Definition: vpWireFrameSimulator.h:196
vpPoint::setWorldCoordinates
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
vpWireFrameSimulator::projectCameraTrajectory
vpImagePoint projectCameraTrajectory(const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
Definition: vpWireFrameSimulator.cpp:1461
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:673
vpWireFrameSimulator::rotz
vpHomogeneousMatrix rotz
Definition: vpWireFrameSimulator.h:245
vpWireFrameSimulator::PIPE
A pipe represented by a cylinder of 25 cm length and 15cm radius.
Definition: vpWireFrameSimulator.h:162
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:175
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
vpDisplay::displayLine
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:337
vpWireFrameSimulator::poseList
std::list< vpHomogeneousMatrix > poseList
Definition: vpWireFrameSimulator.h:214
vpWireFrameSimulator::camMf2
vpHomogeneousMatrix camMf2
Definition: vpWireFrameSimulator.h:226