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