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