Visual Servoing Platform  version 3.6.1 under development (2024-12-06)
vpWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Wire frame simulator
33  *
34 *****************************************************************************/
35 
41 #include <fcntl.h>
42 #include <stdio.h>
43 #include <string.h>
44 #include <vector>
45 #include <visp3/robot/vpWireFrameSimulator.h>
46 
47 #include "vpArit.h"
48 #include "vpBound.h"
49 #include "vpClipping.h"
50 #include "vpCoreDisplay.h"
51 #include "vpKeyword.h"
52 #include "vpLex.h"
53 #include "vpMy.h"
54 #include "vpParser.h"
55 #include "vpProjection.h"
56 #include "vpRfstack.h"
57 #include "vpScene.h"
58 #include "vpTmstack.h"
59 #include "vpToken.h"
60 #include "vpView.h"
61 #include "vpVwstack.h"
62 
63 #include <visp3/core/vpDebug.h>
64 #include <visp3/core/vpCameraParameters.h>
65 #include <visp3/core/vpException.h>
66 #include <visp3/core/vpIoTools.h>
67 #include <visp3/core/vpMeterPixelConversion.h>
68 #include <visp3/core/vpPoint.h>
69 
70 BEGIN_VISP_NAMESPACE
71 extern Point2i *point2i;
72 extern Point2i *listpoint2i;
73 
74 /*
75  Copy the scene corresponding to the registeresd parameters in the image.
76 */
77 void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
78 {
79  // extern Bound *clipping_Bound ();
80  Bound *bp, *bend;
81  Bound *clip; /* surface apres clipping */
82  Byte b = (Byte)*get_rfstack();
83  Matrix m;
84 
85  // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
86  memmove((char *)m, (char *)mat, sizeof(Matrix));
87  View_to_Matrix(get_vwstack(), *(get_tmstack()));
88  postmult_matrix(m, *(get_tmstack()));
89  bp = sc.bound.ptr;
90  bend = bp + sc.bound.nbr;
91  for (; bp < bend; bp++) {
92  if ((clip = clipping_Bound(bp, m)) != NULL) {
93  Face *fp = clip->face.ptr;
94  Face *fend = fp + clip->face.nbr;
95 
96  set_Bound_face_display(clip, b); // regarde si is_visible
97 
98  point_3D_2D(clip->point.ptr, clip->point.nbr, (int)I.getWidth(), (int)I.getHeight(), point2i);
99  for (; fp < fend; fp++) {
100  if (fp->is_visible) {
101  wireframe_Face(fp, point2i);
102  Point2i *pt = listpoint2i;
103  for (int i = 1; i < fp->vertex.nbr; i++) {
104  vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
105  thickness_);
106  pt++;
107  }
108  if (fp->vertex.nbr > 2) {
109  vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
110  color, thickness_);
111  }
112  }
113  }
114  }
115  }
116 }
117 
118 /*
119  Copy the scene corresponding to the registeresd parameters in the image.
120 */
121 void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I,
122  const vpColor &color)
123 {
124  // extern Bound *clipping_Bound ();
125 
126  Bound *bp, *bend;
127  Bound *clip; /* surface apres clipping */
128  Byte b = (Byte)*get_rfstack();
129  Matrix m;
130 
131  // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
132  memmove((char *)m, (char *)mat, sizeof(Matrix));
133  View_to_Matrix(get_vwstack(), *(get_tmstack()));
134  postmult_matrix(m, *(get_tmstack()));
135  bp = sc.bound.ptr;
136  bend = bp + sc.bound.nbr;
137  for (; bp < bend; bp++) {
138  if ((clip = clipping_Bound(bp, m)) != NULL) {
139  Face *fp = clip->face.ptr;
140  Face *fend = fp + clip->face.nbr;
141 
142  set_Bound_face_display(clip, b); // regarde si is_visible
143 
144  point_3D_2D(clip->point.ptr, clip->point.nbr, (int)I.getWidth(), (int)I.getHeight(), point2i);
145  for (; fp < fend; fp++) {
146  if (fp->is_visible) {
147  wireframe_Face(fp, point2i);
148  Point2i *pt = listpoint2i;
149  for (int i = 1; i < fp->vertex.nbr; i++) {
150  vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
151  thickness_);
152  pt++;
153  }
154  if (fp->vertex.nbr > 2) {
155  vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
156  color, thickness_);
157  }
158  }
159  }
160  }
161  }
162 }
163 
164 /*************************************************************************************************************/
165 
175  : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(), refMo(), cMo(), cdMo(), object(PLATE),
176  desiredObject(D_STANDARD), camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
177  desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true), cameraTrajectory(), poseList(),
178  fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(), old_iPt(), blockedr(false), blockedz(false), blockedt(false),
179  blocked(false), camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
180  displayDesiredObject(false), displayCamera(false), displayImageSimulator(false), cameraFactor(1.),
181  camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
182 {
183  // set scene_dir from #define VISP_SCENE_DIR if it exists
184  // VISP_SCENES_DIR may contain multiple locations separated by ";"
185  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
186  bool sceneDirExists = false;
187  for (size_t i = 0; i < scene_dirs.size(); i++)
188  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
189  scene_dir = scene_dirs[i];
190  sceneDirExists = true;
191  break;
192  }
193  if (!sceneDirExists) {
194  try {
195  scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
196  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
197  }
198  catch (...) {
199  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
200  }
201  }
202 
203  open_display();
204  open_clipping();
205 
206  old_iPr = vpImagePoint(-1, -1);
207  old_iPz = vpImagePoint(-1, -1);
208  old_iPt = vpImagePoint(-1, -1);
209 
210  rotz.buildFrom(0, 0, 0, 0, 0, vpMath::rad(180));
211 
212  scene.name = NULL;
213  scene.bound.ptr = NULL;
214  scene.bound.nbr = 0;
215 
216  desiredScene.name = NULL;
217  desiredScene.bound.ptr = NULL;
218  desiredScene.bound.nbr = 0;
219 
220  camera.name = NULL;
221  camera.bound.ptr = NULL;
222  camera.bound.nbr = 0;
223 }
224 
229 {
230  if (sceneInitialized) {
231  if (displayObject)
232  free_Bound_scene(&(this->scene));
233  if (displayCamera) {
234  free_Bound_scene(&(this->camera));
235  }
237  free_Bound_scene(&(this->desiredScene));
238  }
239  close_clipping();
240  close_display();
241 
242  cameraTrajectory.clear();
243  poseList.clear();
244  fMoList.clear();
245 }
246 
262 {
263  char name_cam[FILENAME_MAX];
264  char name[FILENAME_MAX];
265 
266  object = obj;
267  this->desiredObject = desired_object;
268 
269  const char *scene_dir_ = scene_dir.c_str();
270  if (strlen(scene_dir_) >= FILENAME_MAX) {
271  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
272  }
273 
274  strcpy(name_cam, scene_dir_);
275  if (desiredObject != D_TOOL) {
276  strcat(name_cam, "/camera.bnd");
277  set_scene(name_cam, &camera, cameraFactor);
278  }
279  else {
280  strcat(name_cam, "/tool.bnd");
281  set_scene(name_cam, &(this->camera), 1.0);
282  }
283 
284  strcpy(name, scene_dir_);
285  switch (obj) {
286  case THREE_PTS: {
287  strcat(name, "/3pts.bnd");
288  break;
289  }
290  case CUBE: {
291  strcat(name, "/cube.bnd");
292  break;
293  }
294  case PLATE: {
295  strcat(name, "/plate.bnd");
296  break;
297  }
298  case SMALL_PLATE: {
299  strcat(name, "/plate_6cm.bnd");
300  break;
301  }
302  case RECTANGLE: {
303  strcat(name, "/rectangle.bnd");
304  break;
305  }
306  case SQUARE_10CM: {
307  strcat(name, "/square10cm.bnd");
308  break;
309  }
310  case DIAMOND: {
311  strcat(name, "/diamond.bnd");
312  break;
313  }
314  case TRAPEZOID: {
315  strcat(name, "/trapezoid.bnd");
316  break;
317  }
318  case THREE_LINES: {
319  strcat(name, "/line.bnd");
320  break;
321  }
322  case ROAD: {
323  strcat(name, "/road.bnd");
324  break;
325  }
326  case TIRE: {
327  strcat(name, "/circles2.bnd");
328  break;
329  }
330  case PIPE: {
331  strcat(name, "/pipe.bnd");
332  break;
333  }
334  case CIRCLE: {
335  strcat(name, "/circle.bnd");
336  break;
337  }
338  case SPHERE: {
339  strcat(name, "/sphere.bnd");
340  break;
341  }
342  case CYLINDER: {
343  strcat(name, "/cylinder.bnd");
344  break;
345  }
346  case PLAN: {
347  strcat(name, "/plan.bnd");
348  break;
349  }
350  case POINT_CLOUD: {
351  strcat(name, "/point_cloud.bnd");
352  break;
353  }
354  }
355  set_scene(name, &(this->scene), 1.0);
356 
357  scene_dir_ = scene_dir.c_str();
358  if (strlen(scene_dir_) >= FILENAME_MAX) {
359  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the desired object name"));
360  }
361 
362  switch (desiredObject) {
363  case D_STANDARD: {
364  break;
365  }
366  case D_CIRCLE: {
367  strcpy(name, scene_dir_);
368  strcat(name, "/circle_sq2.bnd");
369  break;
370  }
371  case D_TOOL: {
372  strcpy(name, scene_dir_);
373  strcat(name, "/tool.bnd");
374  break;
375  }
376  }
377  set_scene(name, &(this->desiredScene), 1.0);
378 
379  if (obj == PIPE)
380  load_rfstack(IS_INSIDE);
381  else
382  add_rfstack(IS_BACK);
383 
384  add_vwstack("start", "depth", 0.0, 100.0);
385  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
386  add_vwstack("start", "type", PERSPECTIVE);
387 
388  sceneInitialized = true;
389  displayObject = true;
390  displayDesiredObject = true;
391  displayCamera = true;
392  displayImageSimulator = true;
393 }
394 
415  const std::list<vpImageSimulator> &imObj)
416 {
417  initScene(obj, desired_object);
418  objectImage = imObj;
419  displayImageSimulator = true;
420 }
421 
433 void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object)
434 {
435  char name_cam[FILENAME_MAX];
436  char name[FILENAME_MAX];
437 
438  object = THREE_PTS;
439  this->desiredObject = D_STANDARD;
440 
441  const char *scene_dir_ = scene_dir.c_str();
442  if (strlen(scene_dir_) >= FILENAME_MAX) {
443  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
444  }
445 
446  strcpy(name_cam, scene_dir_);
447  strcat(name_cam, "/camera.bnd");
448  set_scene(name_cam, &camera, cameraFactor);
449 
450  if (strlen(obj) >= FILENAME_MAX) {
451  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the name"));
452  }
453 
454  strcpy(name, obj);
455  Model_3D model;
456  model = getExtension(obj);
457  if (model == BND_MODEL)
458  set_scene(name, &(this->scene), 1.0);
459  else if (model == WRL_MODEL)
460  set_scene_wrl(name, &(this->scene), 1.0);
461  else if (model == UNKNOWN_MODEL) {
462  vpERROR_TRACE("Unknown file extension for the 3D model");
463  }
464 
465  if (strlen(desired_object) >= FILENAME_MAX) {
466  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
467  }
468 
469  strcpy(name, desired_object);
470  model = getExtension(desired_object);
471  if (model == BND_MODEL)
472  set_scene(name, &(this->desiredScene), 1.0);
473  else if (model == WRL_MODEL)
474  set_scene_wrl(name, &(this->desiredScene), 1.0);
475  else if (model == UNKNOWN_MODEL) {
476  vpERROR_TRACE("Unknown file extension for the 3D model");
477  }
478 
479  add_rfstack(IS_BACK);
480 
481  add_vwstack("start", "depth", 0.0, 100.0);
482  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
483  add_vwstack("start", "type", PERSPECTIVE);
484 
485  sceneInitialized = true;
486  displayObject = true;
487  displayDesiredObject = true;
488  displayCamera = true;
489 }
490 
507 void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object,
508  const std::list<vpImageSimulator> &imObj)
509 {
510  initScene(obj, desired_object);
511  objectImage = imObj;
512  displayImageSimulator = true;
513 }
514 
528 {
529  char name_cam[FILENAME_MAX];
530  char name[FILENAME_MAX];
531 
532  object = obj;
533 
534  const char *scene_dir_ = scene_dir.c_str();
535  if (strlen(scene_dir_) >= FILENAME_MAX) {
536  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
537  }
538 
539  strcpy(name_cam, scene_dir_);
540  strcat(name_cam, "/camera.bnd");
541  set_scene(name_cam, &camera, cameraFactor);
542 
543  strcpy(name, scene_dir_);
544  switch (obj) {
545  case THREE_PTS: {
546  strcat(name, "/3pts.bnd");
547  break;
548  }
549  case CUBE: {
550  strcat(name, "/cube.bnd");
551  break;
552  }
553  case PLATE: {
554  strcat(name, "/plate.bnd");
555  break;
556  }
557  case SMALL_PLATE: {
558  strcat(name, "/plate_6cm.bnd");
559  break;
560  }
561  case RECTANGLE: {
562  strcat(name, "/rectangle.bnd");
563  break;
564  }
565  case SQUARE_10CM: {
566  strcat(name, "/square10cm.bnd");
567  break;
568  }
569  case DIAMOND: {
570  strcat(name, "/diamond.bnd");
571  break;
572  }
573  case TRAPEZOID: {
574  strcat(name, "/trapezoid.bnd");
575  break;
576  }
577  case THREE_LINES: {
578  strcat(name, "/line.bnd");
579  break;
580  }
581  case ROAD: {
582  strcat(name, "/road.bnd");
583  break;
584  }
585  case TIRE: {
586  strcat(name, "/circles2.bnd");
587  break;
588  }
589  case PIPE: {
590  strcat(name, "/pipe.bnd");
591  break;
592  }
593  case CIRCLE: {
594  strcat(name, "/circle.bnd");
595  break;
596  }
597  case SPHERE: {
598  strcat(name, "/sphere.bnd");
599  break;
600  }
601  case CYLINDER: {
602  strcat(name, "/cylinder.bnd");
603  break;
604  }
605  case PLAN: {
606  strcat(name, "/plan.bnd");
607  break;
608  }
609  case POINT_CLOUD: {
610  strcat(name, "/point_cloud.bnd");
611  break;
612  }
613  }
614  set_scene(name, &(this->scene), 1.0);
615 
616  if (obj == PIPE)
617  load_rfstack(IS_INSIDE);
618  else
619  add_rfstack(IS_BACK);
620 
621  add_vwstack("start", "depth", 0.0, 100.0);
622  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
623  add_vwstack("start", "type", PERSPECTIVE);
624 
625  sceneInitialized = true;
626  displayObject = true;
627  displayCamera = true;
628 
629  displayDesiredObject = false;
630  displayImageSimulator = false;
631 }
632 
649 void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
650 {
651  initScene(obj);
652  objectImage = imObj;
653  displayImageSimulator = true;
654 }
655 
666 void vpWireFrameSimulator::initScene(const char *obj)
667 {
668  char name_cam[FILENAME_MAX];
669  char name[FILENAME_MAX];
670 
671  object = THREE_PTS;
672 
673  const char *scene_dir_ = scene_dir.c_str();
674  if (strlen(scene_dir_) >= FILENAME_MAX) {
675  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
676  }
677 
678  strcpy(name_cam, scene_dir_);
679  strcat(name_cam, "/camera.bnd");
680  set_scene(name_cam, &camera, cameraFactor);
681 
682  if (strlen(obj) >= FILENAME_MAX) {
683  throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the name"));
684  }
685 
686  strcpy(name, obj);
687  Model_3D model;
688  model = getExtension(obj);
689  if (model == BND_MODEL)
690  set_scene(name, &(this->scene), 1.0);
691  else if (model == WRL_MODEL)
692  set_scene_wrl(name, &(this->scene), 1.0);
693  else if (model == UNKNOWN_MODEL) {
694  vpERROR_TRACE("Unknown file extension for the 3D model");
695  }
696 
697  add_rfstack(IS_BACK);
698 
699  add_vwstack("start", "depth", 0.0, 100.0);
700  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
701  add_vwstack("start", "type", PERSPECTIVE);
702 
703  sceneInitialized = true;
704  displayObject = true;
705  displayCamera = true;
706 }
707 
723 void vpWireFrameSimulator::initScene(const char *obj, const std::list<vpImageSimulator> &imObj)
724 {
725  initScene(obj);
726  objectImage = imObj;
727  displayImageSimulator = true;
728 }
729 
739 {
740  if (!sceneInitialized)
741  throw(vpException(vpException::notInitialized, "The scene has to be initialized"));
742 
743  double u;
744  double v;
745  // if(px_int != 1 && py_int != 1)
746  // we assume px_int and py_int > 0
747  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
748  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
749  u = (double)I.getWidth() / (2 * px_int);
750  v = (double)I.getHeight() / (2 * py_int);
751  }
752  else {
753  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
754  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
755  }
756 
757  float o44c[4][4], o44cd[4][4], x, y, z;
758  Matrix id = IDENTITY_MATRIX;
759 
760  vp2jlc_matrix(cMo.inverse(), o44c);
761  vp2jlc_matrix(cdMo.inverse(), o44cd);
762 
763  if (displayImageSimulator) {
764  I = vpRGBa(255);
765 
766  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
767  vpImageSimulator *imSim = &(*it);
768  imSim->setCameraPosition(rotz * cMo);
770  }
771 
772  if (I.display != NULL)
774  }
775 
776  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
777  x = o44c[2][0] + o44c[3][0];
778  y = o44c[2][1] + o44c[3][1];
779  z = o44c[2][2] + o44c[3][2];
780  add_vwstack("start", "vrp", x, y, z);
781  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
782  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
783  add_vwstack("start", "window", -u, u, -v, v);
784  if (displayObject)
785  display_scene(id, this->scene, I, curColor);
786 
787  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
788  x = o44cd[2][0] + o44cd[3][0];
789  y = o44cd[2][1] + o44cd[3][1];
790  z = o44cd[2][2] + o44cd[3][2];
791  add_vwstack("start", "vrp", x, y, z);
792  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
793  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
794  add_vwstack("start", "window", -u, u, -v, v);
795  if (displayDesiredObject) {
796  if (desiredObject == D_TOOL)
798  else
800  }
801 }
802 
814 {
815  bool changed = false;
816  vpHomogeneousMatrix displacement = navigation(I, changed);
817 
818  // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
819  // 0*/)
820  if (std::fabs(displacement[2][3]) >
821  std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
822  camMf2 = camMf2 * displacement;
823 
824  f2Mf = camMf2.inverse() * camMf;
825 
826  camMf = camMf2 * displacement * f2Mf;
827 
828  double u;
829  double v;
830  // if(px_ext != 1 && py_ext != 1)
831  // we assume px_ext and py_ext > 0
832  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
833  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
834  u = (double)I.getWidth() / (2 * px_ext);
835  v = (double)I.getHeight() / (2 * py_ext);
836  }
837  else {
838  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
839  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
840  }
841 
842  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
843 
844  vp2jlc_matrix(camMf.inverse(), w44cext);
845  vp2jlc_matrix(fMc, w44c);
846  vp2jlc_matrix(fMo, w44o);
847 
848  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
849  x = w44cext[2][0] + w44cext[3][0];
850  y = w44cext[2][1] + w44cext[3][1];
851  z = w44cext[2][2] + w44cext[3][2];
852  add_vwstack("start", "vrp", x, y, z);
853  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
854  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
855  add_vwstack("start", "window", -u, u, -v, v);
856  if ((object == CUBE) || (object == SPHERE)) {
857  add_vwstack("start", "type", PERSPECTIVE);
858  }
859 
860  if (displayImageSimulator) {
861  I = vpRGBa(255);
862 
863  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
864  vpImageSimulator *imSim = &(*it);
865  imSim->setCameraPosition(rotz * camMf * fMo);
867  }
868 
869  if (I.display != NULL)
871  }
872 
873  if (displayObject)
874  display_scene(w44o, this->scene, I, curColor);
875 
876  if (displayCamera)
877  display_scene(w44c, camera, I, camColor);
878 
880  vpImagePoint iP;
881  vpImagePoint iP_1;
882  poseList.push_back(cMo);
883  fMoList.push_back(fMo);
884 
885  int iter = 0;
886 
887  if (changed || extCamChanged) {
888  cameraTrajectory.clear();
889  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
890  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
891 
892  while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
893  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
894  cameraTrajectory.push_back(iP);
895  if (camTrajType == CT_LINE) {
896  if (iter != 0)
898  }
899  else if (camTrajType == CT_POINT)
901  ++iter_poseList;
902  ++iter_fMoList;
903  iter++;
904  iP_1 = iP;
905  }
906  extCamChanged = false;
907  }
908  else {
909  iP = projectCameraTrajectory(I, cMo, fMo);
910  cameraTrajectory.push_back(iP);
911 
912  for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
913  if (camTrajType == CT_LINE) {
914  if (iter != 0)
916  }
917  else if (camTrajType == CT_POINT)
919  iter++;
920  iP_1 = *it;
921  }
922  }
923 
924  if (poseList.size() > nbrPtLimit) {
925  poseList.pop_front();
926  }
927  if (fMoList.size() > nbrPtLimit) {
928  fMoList.pop_front();
929  }
930  if (cameraTrajectory.size() > nbrPtLimit) {
931  cameraTrajectory.pop_front();
932  }
933  }
934 }
935 
948 {
949  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
950 
951  vpHomogeneousMatrix camMft = rotz * cam_Mf;
952 
953  double u;
954  double v;
955  // if(px_ext != 1 && py_ext != 1)
956  // we assume px_ext and py_ext > 0
957  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
958  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
959  u = (double)I.getWidth() / (2 * px_ext);
960  v = (double)I.getHeight() / (2 * py_ext);
961  }
962  else {
963  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
964  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
965  }
966 
967  vp2jlc_matrix(camMft.inverse(), w44cext);
968  vp2jlc_matrix(fMo * cMo.inverse(), w44c);
969  vp2jlc_matrix(fMo, w44o);
970 
971  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
972  x = w44cext[2][0] + w44cext[3][0];
973  y = w44cext[2][1] + w44cext[3][1];
974  z = w44cext[2][2] + w44cext[3][2];
975  add_vwstack("start", "vrp", x, y, z);
976  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
977  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
978  add_vwstack("start", "window", -u, u, -v, v);
979 
980  if (displayImageSimulator) {
981  I = vpRGBa(255);
982 
983  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
984  vpImageSimulator *imSim = &(*it);
985  imSim->setCameraPosition(rotz * cam_Mf * fMo);
987  }
988 
989  if (I.display != NULL)
991  }
992 
993  if (displayObject)
994  display_scene(w44o, this->scene, I, curColor);
995  if (displayCamera)
996  display_scene(w44c, camera, I, camColor);
997 }
998 
1008 {
1009  if (!sceneInitialized)
1010  throw(vpException(vpException::notInitialized, "The scene has to be initialized"));
1011 
1012  double u;
1013  double v;
1014  // if(px_int != 1 && py_int != 1)
1015  // we assume px_int and py_int > 0
1016  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
1017  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
1018  u = (double)I.getWidth() / (2 * px_int);
1019  v = (double)I.getHeight() / (2 * py_int);
1020  }
1021  else {
1022  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1023  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1024  }
1025 
1026  float o44c[4][4], o44cd[4][4], x, y, z;
1027  Matrix id = IDENTITY_MATRIX;
1028 
1029  vp2jlc_matrix(cMo.inverse(), o44c);
1030  vp2jlc_matrix(cdMo.inverse(), o44cd);
1031 
1032  if (displayImageSimulator) {
1033  I = 255u;
1034 
1035  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
1036  vpImageSimulator *imSim = &(*it);
1037  imSim->setCameraPosition(rotz * camMf * fMo);
1038  imSim->getImage(I, getInternalCameraParameters(I));
1039  }
1040 
1041  if (I.display != NULL)
1042  vpDisplay::display(I);
1043  }
1044 
1045  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
1046  x = o44c[2][0] + o44c[3][0];
1047  y = o44c[2][1] + o44c[3][1];
1048  z = o44c[2][2] + o44c[3][2];
1049  add_vwstack("start", "vrp", x, y, z);
1050  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
1051  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
1052  add_vwstack("start", "window", -u, u, -v, v);
1053  if (displayObject)
1054  display_scene(id, this->scene, I, curColor);
1055 
1056  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
1057  x = o44cd[2][0] + o44cd[3][0];
1058  y = o44cd[2][1] + o44cd[3][1];
1059  z = o44cd[2][2] + o44cd[3][2];
1060  add_vwstack("start", "vrp", x, y, z);
1061  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
1062  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
1063  add_vwstack("start", "window", -u, u, -v, v);
1064  if (displayDesiredObject) {
1065  if (desiredObject == D_TOOL)
1067  else
1069  }
1070 }
1071 
1083 {
1084  bool changed = false;
1085  vpHomogeneousMatrix displacement = navigation(I, changed);
1086 
1087  // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
1088  // 0*/)
1089  if (std::fabs(displacement[2][3]) >
1090  std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1091  camMf2 = camMf2 * displacement;
1092 
1093  f2Mf = camMf2.inverse() * camMf;
1094 
1095  camMf = camMf2 * displacement * f2Mf;
1096 
1097  double u;
1098  double v;
1099  // if(px_ext != 1 && py_ext != 1)
1100  // we assume px_ext and py_ext > 0
1101  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
1102  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
1103  u = (double)I.getWidth() / (2 * px_ext);
1104  v = (double)I.getHeight() / (2 * py_ext);
1105  }
1106  else {
1107  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1108  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1109  }
1110 
1111  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1112 
1113  vp2jlc_matrix(camMf.inverse(), w44cext);
1114  vp2jlc_matrix(fMc, w44c);
1115  vp2jlc_matrix(fMo, w44o);
1116 
1117  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
1118  x = w44cext[2][0] + w44cext[3][0];
1119  y = w44cext[2][1] + w44cext[3][1];
1120  z = w44cext[2][2] + w44cext[3][2];
1121  add_vwstack("start", "vrp", x, y, z);
1122  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
1123  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
1124  add_vwstack("start", "window", -u, u, -v, v);
1125  if ((object == CUBE) || (object == SPHERE)) {
1126  add_vwstack("start", "type", PERSPECTIVE);
1127  }
1128 
1129  if (displayImageSimulator) {
1130  I = 255u;
1131  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
1132  vpImageSimulator *imSim = &(*it);
1133  imSim->setCameraPosition(rotz * camMf * fMo);
1134  imSim->getImage(I, getInternalCameraParameters(I));
1135  }
1136  if (I.display != NULL)
1137  vpDisplay::display(I);
1138  }
1139 
1140  if (displayObject)
1141  display_scene(w44o, this->scene, I, curColor);
1142 
1143  if (displayCamera)
1144  display_scene(w44c, camera, I, camColor);
1145 
1147  vpImagePoint iP;
1148  vpImagePoint iP_1;
1149  poseList.push_back(cMo);
1150  fMoList.push_back(fMo);
1151 
1152  int iter = 0;
1153 
1154  if (changed || extCamChanged) {
1155  cameraTrajectory.clear();
1156  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
1157  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
1158 
1159  while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
1160  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
1161  cameraTrajectory.push_back(iP);
1162  // vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
1163  if (camTrajType == CT_LINE) {
1164  if (iter != 0)
1166  }
1167  else if (camTrajType == CT_POINT)
1169  ++iter_poseList;
1170  ++iter_fMoList;
1171  iter++;
1172  iP_1 = iP;
1173  }
1174  extCamChanged = false;
1175  }
1176  else {
1177  iP = projectCameraTrajectory(I, cMo, fMo);
1178  cameraTrajectory.push_back(iP);
1179 
1180  for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
1181  if (camTrajType == CT_LINE) {
1182  if (iter != 0)
1184  }
1185  else if (camTrajType == CT_POINT)
1187  iter++;
1188  iP_1 = *it;
1189  }
1190  }
1191 
1192  if (poseList.size() > nbrPtLimit) {
1193  poseList.pop_front();
1194  }
1195  if (fMoList.size() > nbrPtLimit) {
1196  fMoList.pop_front();
1197  }
1198  if (cameraTrajectory.size() > nbrPtLimit) {
1199  cameraTrajectory.pop_front();
1200  }
1201  }
1202 }
1203 
1216 {
1217  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1218 
1219  vpHomogeneousMatrix camMft = rotz * cam_Mf;
1220 
1221  double u;
1222  double v;
1223  // if(px_ext != 1 && py_ext != 1)
1224  // we assume px_ext and py_ext > 0
1225  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
1226  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
1227  u = (double)I.getWidth() / (2 * px_ext);
1228  v = (double)I.getHeight() / (2 * py_ext);
1229  }
1230  else {
1231  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1232  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1233  }
1234 
1235  vp2jlc_matrix(camMft.inverse(), w44cext);
1236  vp2jlc_matrix(fMo * cMo.inverse(), w44c);
1237  vp2jlc_matrix(fMo, w44o);
1238 
1239  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
1240  x = w44cext[2][0] + w44cext[3][0];
1241  y = w44cext[2][1] + w44cext[3][1];
1242  z = w44cext[2][2] + w44cext[3][2];
1243  add_vwstack("start", "vrp", x, y, z);
1244  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
1245  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
1246  add_vwstack("start", "window", -u, u, -v, v);
1247 
1248  if (displayImageSimulator) {
1249  I = 255u;
1250  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
1251  vpImageSimulator *imSim = &(*it);
1252  imSim->setCameraPosition(rotz * cam_Mf * fMo);
1253  imSim->getImage(I, getInternalCameraParameters(I));
1254  }
1255  if (I.display != NULL)
1256  vpDisplay::display(I);
1257  }
1258 
1259  if (displayObject)
1260  display_scene(w44o, this->scene, I, curColor);
1261  if (displayCamera)
1262  display_scene(w44c, camera, I, camColor);
1263 }
1264 
1283  const std::list<vpHomogeneousMatrix> &list_cMo,
1284  const std::list<vpHomogeneousMatrix> &list_fMo,
1285  const vpHomogeneousMatrix &cMf)
1286 {
1287  if (list_cMo.size() != list_fMo.size())
1288  throw(vpException(vpException::dimensionError, "The two lists must have the same size"));
1289 
1290  vpImagePoint iP;
1291  vpImagePoint iP_1;
1292  int iter = 0;
1293 
1294  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1295  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1296 
1297  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1298  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1299  if (camTrajType == CT_LINE) {
1300  if (iter != 0)
1302  }
1303  else if (camTrajType == CT_POINT)
1305  ++it_cMo;
1306  ++it_fMo;
1307  iter++;
1308  iP_1 = iP;
1309  }
1310 }
1311 
1329 void vpWireFrameSimulator::displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
1330  const std::list<vpHomogeneousMatrix> &list_fMo,
1331  const vpHomogeneousMatrix &cMf)
1332 {
1333  if (list_cMo.size() != list_fMo.size())
1334  throw(vpException(vpException::dimensionError, "The two lists must have the same size"));
1335 
1336  vpImagePoint iP;
1337  vpImagePoint iP_1;
1338  int iter = 0;
1339 
1340  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1341  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1342 
1343  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1344  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1345  if (camTrajType == CT_LINE) {
1346  if (iter != 0)
1348  }
1349  else if (camTrajType == CT_POINT)
1351  ++it_cMo;
1352  ++it_fMo;
1353  iter++;
1354  iP_1 = iP;
1355  }
1356 }
1357 
1362 {
1363  double width = vpMath::minimum(I.getWidth(), I.getHeight());
1364  vpImagePoint iP;
1365  vpImagePoint trash;
1366  bool clicked = false;
1367  bool clickedUp = false;
1369 
1370  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
1371  changed = false;
1372 
1373  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1374 
1375  if (!blocked)
1376  clicked = vpDisplay::getClick(I, trash, b, false);
1377 
1378  if (blocked)
1379  clickedUp = vpDisplay::getClickUp(I, trash, b, false);
1380 
1381  if (clicked) {
1382  if (b == vpMouseButton::button1)
1383  blockedr = true;
1384  if (b == vpMouseButton::button2)
1385  blockedz = true;
1386  if (b == vpMouseButton::button3)
1387  blockedt = true;
1388  blocked = true;
1389  }
1390  if (clickedUp) {
1391  if (b == vpMouseButton::button1) {
1392  old_iPr = vpImagePoint(-1, -1);
1393  blockedr = false;
1394  }
1395  if (b == vpMouseButton::button2) {
1396  old_iPz = vpImagePoint(-1, -1);
1397  blockedz = false;
1398  }
1399  if (b == vpMouseButton::button3) {
1400  old_iPt = vpImagePoint(-1, -1);
1401  blockedt = false;
1402  }
1403  if (!(blockedr || blockedz || blockedt)) {
1404  blocked = false;
1405  while (vpDisplay::getClick(I, trash, b, false)) {
1406  };
1407  }
1408  }
1409 
1411 
1412  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
1413  double diffi = iP.get_i() - old_iPr.get_i();
1414  double diffj = iP.get_j() - old_iPr.get_j();
1415  // cout << "delta :" << diffj << endl;;
1416  double anglei = diffi * 360 / width;
1417  double anglej = diffj * 360 / width;
1418  mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
1419  changed = true;
1420  }
1421 
1422  if (blockedr)
1423  old_iPr = iP;
1424 
1425  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
1426  double diffi = iP.get_i() - old_iPz.get_i();
1427  mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1428  changed = true;
1429  }
1430 
1431  if (blockedz)
1432  old_iPz = iP;
1433 
1434  if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
1435  double diffi = iP.get_i() - old_iPt.get_i();
1436  double diffj = iP.get_j() - old_iPt.get_j();
1437  mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
1438  changed = true;
1439  }
1440 
1441  if (blockedt)
1442  old_iPt = iP;
1443 
1444  return mov;
1445 }
1446 
1451 {
1452  double width = vpMath::minimum(I.getWidth(), I.getHeight());
1453  vpImagePoint iP;
1454  vpImagePoint trash;
1455  bool clicked = false;
1456  bool clickedUp = false;
1458 
1459  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
1460  changed = false;
1461 
1462  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1463 
1464  if (!blocked)
1465  clicked = vpDisplay::getClick(I, trash, b, false);
1466 
1467  if (blocked)
1468  clickedUp = vpDisplay::getClickUp(I, trash, b, false);
1469 
1470  if (clicked) {
1471  if (b == vpMouseButton::button1)
1472  blockedr = true;
1473  if (b == vpMouseButton::button2)
1474  blockedz = true;
1475  if (b == vpMouseButton::button3)
1476  blockedt = true;
1477  blocked = true;
1478  }
1479  if (clickedUp) {
1480  if (b == vpMouseButton::button1) {
1481  old_iPr = vpImagePoint(-1, -1);
1482  blockedr = false;
1483  }
1484  if (b == vpMouseButton::button2) {
1485  old_iPz = vpImagePoint(-1, -1);
1486  blockedz = false;
1487  }
1488  if (b == vpMouseButton::button3) {
1489  old_iPt = vpImagePoint(-1, -1);
1490  blockedt = false;
1491  }
1492  if (!(blockedr || blockedz || blockedt)) {
1493  blocked = false;
1494  while (vpDisplay::getClick(I, trash, b, false)) {
1495  };
1496  }
1497  }
1498 
1500 
1501  // std::cout << "point : " << iP << std::endl;
1502 
1503  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
1504  double diffi = iP.get_i() - old_iPr.get_i();
1505  double diffj = iP.get_j() - old_iPr.get_j();
1506  // cout << "delta :" << diffj << endl;;
1507  double anglei = diffi * 360 / width;
1508  double anglej = diffj * 360 / width;
1509  mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
1510  changed = true;
1511  }
1512 
1513  if (blockedr)
1514  old_iPr = iP;
1515 
1516  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
1517  double diffi = iP.get_i() - old_iPz.get_i();
1518  mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1519  changed = true;
1520  }
1521 
1522  if (blockedz)
1523  old_iPz = iP;
1524 
1525  if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
1526  double diffi = iP.get_i() - old_iPt.get_i();
1527  double diffj = iP.get_j() - old_iPt.get_j();
1528  mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
1529  changed = true;
1530  }
1531 
1532  if (blockedt)
1533  old_iPt = iP;
1534 
1535  return mov;
1536 }
1537 
1542  const vpHomogeneousMatrix &fMo_)
1543 {
1544  vpPoint point;
1545  point.setWorldCoordinates(0, 0, 0);
1546 
1547  point.track(rotz * (camMf * fMo_ * cMo_.inverse()));
1548 
1549  vpImagePoint iP;
1550 
1552 
1553  return iP;
1554 }
1555 
1560  const vpHomogeneousMatrix &cMo_,
1561  const vpHomogeneousMatrix &fMo_)
1562 {
1563  vpPoint point;
1564  point.setWorldCoordinates(0, 0, 0);
1565 
1566  point.track(rotz * (camMf * fMo_ * cMo_.inverse()));
1567 
1568  vpImagePoint iP;
1569 
1571 
1572  return iP;
1573 }
1574 
1579  const vpHomogeneousMatrix &fMo_,
1580  const vpHomogeneousMatrix &cMf)
1581 {
1582  vpPoint point;
1583  point.setWorldCoordinates(0, 0, 0);
1584 
1585  point.track(rotz * (cMf * fMo_ * cMo_.inverse()));
1586 
1587  vpImagePoint iP;
1588 
1590 
1591  return iP;
1592 }
1593 
1598  const vpHomogeneousMatrix &cMo_,
1599  const vpHomogeneousMatrix &fMo_,
1600  const vpHomogeneousMatrix &cMf)
1601 {
1602  vpPoint point;
1603  point.setWorldCoordinates(0, 0, 0);
1604 
1605  point.track(rotz * (cMf * fMo_ * cMo_.inverse()));
1606 
1607  vpImagePoint iP;
1608 
1610 
1611  return iP;
1612 }
1613 END_VISP_NAMESPACE
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static bool getClickUp(const vpImage< unsigned char > &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static bool getPointerPosition(const vpImage< unsigned char > &I, vpImagePoint &ip)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:74
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ memoryAllocationError
Memory allocation error.
Definition: vpException.h:64
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_j() const
Definition: vpImagePoint.h:125
double get_i() const
Definition: vpImagePoint.h:114
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
vpDisplay * display
Definition: vpImage.h:136
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1661
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:396
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:326
static double rad(double deg)
Definition: vpMath.h:129
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:262
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:426
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:424
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
Definition: vpRGBa.h:65
vpSceneDesiredObject desiredObject
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
@ D_CIRCLE
The object displayed at the desired position is a circle.
@ D_TOOL
A cylindrical tool is attached to the camera.
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
void getInternalImage(vpImage< unsigned char > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
vpCameraTrajectoryDisplayType camTrajType
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
void displayTrajectory(const vpImage< unsigned char > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
vpHomogeneousMatrix fMo
vpHomogeneousMatrix camMf
vpHomogeneousMatrix fMc
std::list< vpImagePoint > cameraTrajectory
vpHomogeneousMatrix cMo
std::list< vpHomogeneousMatrix > fMoList
std::list< vpImageSimulator > objectImage
vpImagePoint projectCameraTrajectory(const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
@ CIRCLE
A 10cm radius circle.
@ THREE_LINES
Three parallel lines with equation y=-5, y=0, y=5.
@ ROAD
Three parallel lines representing a road.
@ SPHERE
A 15cm radius sphere.
@ CUBE
A 12.5cm size cube.
@ TIRE
A tire represented by 2 circles with radius 10cm and 15cm.
@ CYLINDER
A cylinder of 80cm length and 10cm radius.
vpHomogeneousMatrix rotz
void getExternalImage(vpImage< unsigned char > &I)
vpHomogeneousMatrix cdMo
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
std::list< vpHomogeneousMatrix > poseList