Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpWireFrameSimulator.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
44 #include <fcntl.h>
45 #include <stdio.h>
46 #include <string.h>
47 #include <vector>
48 #include <visp3/robot/vpWireFrameSimulator.h>
49 
50 #include "vpArit.h"
51 #include "vpBound.h"
52 #include "vpClipping.h"
53 #include "vpCoreDisplay.h"
54 #include "vpKeyword.h"
55 #include "vpLex.h"
56 #include "vpMy.h"
57 #include "vpParser.h"
58 #include "vpProjection.h"
59 #include "vpRfstack.h"
60 #include "vpScene.h"
61 #include "vpTmstack.h"
62 #include "vpToken.h"
63 #include "vpView.h"
64 #include "vpVwstack.h"
65 
66 #include <visp3/core/vpCameraParameters.h>
67 #include <visp3/core/vpException.h>
68 #include <visp3/core/vpIoTools.h>
69 #include <visp3/core/vpMeterPixelConversion.h>
70 #include <visp3/core/vpPoint.h>
71 
72 extern Point2i *point2i;
73 extern Point2i *listpoint2i;
74 
75 /*
76  Copy the scene corresponding to the registeresd parameters in the image.
77 */
78 void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
79 {
80  // extern Bound *clipping_Bound ();
81  Bound *bp, *bend;
82  Bound *clip; /* surface apres clipping */
83  Byte b = (Byte)*get_rfstack();
84  Matrix m;
85 
86  // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
87  memmove((char *)m, (char *)mat, sizeof(Matrix));
88  View_to_Matrix(get_vwstack(), *(get_tmstack()));
89  postmult_matrix(m, *(get_tmstack()));
90  bp = sc.bound.ptr;
91  bend = bp + sc.bound.nbr;
92  for (; bp < bend; bp++) {
93  if ((clip = clipping_Bound(bp, m)) != NULL) {
94  Face *fp = clip->face.ptr;
95  Face *fend = fp + clip->face.nbr;
96 
97  set_Bound_face_display(clip, b); // regarde si is_visible
98 
99  point_3D_2D(clip->point.ptr, clip->point.nbr, (int)I.getWidth(), (int)I.getHeight(), point2i);
100  for (; fp < fend; fp++) {
101  if (fp->is_visible) {
102  wireframe_Face(fp, point2i);
103  Point2i *pt = listpoint2i;
104  for (int i = 1; i < fp->vertex.nbr; i++) {
105  vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
106  thickness_);
107  pt++;
108  }
109  if (fp->vertex.nbr > 2) {
110  vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
111  color, thickness_);
112  }
113  }
114  }
115  }
116  }
117 }
118 
119 /*
120  Copy the scene corresponding to the registeresd parameters in the image.
121 */
122 void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I,
123  const vpColor &color)
124 {
125  // extern Bound *clipping_Bound ();
126 
127  Bound *bp, *bend;
128  Bound *clip; /* surface apres clipping */
129  Byte b = (Byte)*get_rfstack();
130  Matrix m;
131 
132  // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
133  memmove((char *)m, (char *)mat, sizeof(Matrix));
134  View_to_Matrix(get_vwstack(), *(get_tmstack()));
135  postmult_matrix(m, *(get_tmstack()));
136  bp = sc.bound.ptr;
137  bend = bp + sc.bound.nbr;
138  for (; bp < bend; bp++) {
139  if ((clip = clipping_Bound(bp, m)) != NULL) {
140  Face *fp = clip->face.ptr;
141  Face *fend = fp + clip->face.nbr;
142 
143  set_Bound_face_display(clip, b); // regarde si is_visible
144 
145  point_3D_2D(clip->point.ptr, clip->point.nbr, (int)I.getWidth(), (int)I.getHeight(), point2i);
146  for (; fp < fend; fp++) {
147  if (fp->is_visible) {
148  wireframe_Face(fp, point2i);
149  Point2i *pt = listpoint2i;
150  for (int i = 1; i < fp->vertex.nbr; i++) {
151  vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
152  thickness_);
153  pt++;
154  }
155  if (fp->vertex.nbr > 2) {
156  vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
157  color, thickness_);
158  }
159  }
160  }
161  }
162  }
163 }
164 
165 /*************************************************************************************************************/
166 
176  : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(), refMo(), cMo(), cdMo(), object(PLATE),
179  fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(), old_iPt(), blockedr(false), blockedz(false), blockedt(false),
180  blocked(false), camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
182  camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
183 {
184  // set scene_dir from #define VISP_SCENE_DIR if it exists
185  // VISP_SCENES_DIR may contain multiple locations separated by ";"
186  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
187  bool sceneDirExists = false;
188  for (size_t i = 0; i < scene_dirs.size(); i++)
189  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
190  scene_dir = scene_dirs[i];
191  sceneDirExists = true;
192  break;
193  }
194  if (!sceneDirExists) {
195  try {
196  scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
197  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
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 intialize 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  } 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 intialize 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 intialize 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 intialize 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 intialize 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 intialize 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 intialize 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 intialize 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  } else {
751  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
752  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
753  }
754 
755  float o44c[4][4], o44cd[4][4], x, y, z;
756  Matrix id = IDENTITY_MATRIX;
757 
758  vp2jlc_matrix(cMo.inverse(), o44c);
759  vp2jlc_matrix(cdMo.inverse(), o44cd);
760 
761  if (displayImageSimulator) {
762  I = 255;
763 
764  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
765  vpImageSimulator *imSim = &(*it);
766  imSim->setCameraPosition(rotz * cMo);
768  }
769 
770  if (I.display != NULL)
772  }
773 
774  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
775  x = o44c[2][0] + o44c[3][0];
776  y = o44c[2][1] + o44c[3][1];
777  z = o44c[2][2] + o44c[3][2];
778  add_vwstack("start", "vrp", x, y, z);
779  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
780  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
781  add_vwstack("start", "window", -u, u, -v, v);
782  if (displayObject)
783  display_scene(id, this->scene, I, curColor);
784 
785  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
786  x = o44cd[2][0] + o44cd[3][0];
787  y = o44cd[2][1] + o44cd[3][1];
788  z = o44cd[2][2] + o44cd[3][2];
789  add_vwstack("start", "vrp", x, y, z);
790  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
791  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
792  add_vwstack("start", "window", -u, u, -v, v);
793  if (displayDesiredObject) {
794  if (desiredObject == D_TOOL)
796  else
798  }
799 }
800 
812 {
813  bool changed = false;
814  vpHomogeneousMatrix displacement = navigation(I, changed);
815 
816  // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
817  // 0*/)
818  if (std::fabs(displacement[2][3]) >
819  std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
820  camMf2 = camMf2 * displacement;
821 
822  f2Mf = camMf2.inverse() * camMf;
823 
824  camMf = camMf2 * displacement * f2Mf;
825 
826  double u;
827  double v;
828  // if(px_ext != 1 && py_ext != 1)
829  // we assume px_ext and py_ext > 0
830  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
831  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
832  u = (double)I.getWidth() / (2 * px_ext);
833  v = (double)I.getHeight() / (2 * py_ext);
834  } else {
835  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
836  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
837  }
838 
839  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
840 
841  vp2jlc_matrix(camMf.inverse(), w44cext);
842  vp2jlc_matrix(fMc, w44c);
843  vp2jlc_matrix(fMo, w44o);
844 
845  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
846  x = w44cext[2][0] + w44cext[3][0];
847  y = w44cext[2][1] + w44cext[3][1];
848  z = w44cext[2][2] + w44cext[3][2];
849  add_vwstack("start", "vrp", x, y, z);
850  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
851  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
852  add_vwstack("start", "window", -u, u, -v, v);
853  if ((object == CUBE) || (object == SPHERE)) {
854  add_vwstack("start", "type", PERSPECTIVE);
855  }
856 
857  if (displayImageSimulator) {
858  I = 255;
859 
860  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
861  vpImageSimulator *imSim = &(*it);
862  imSim->setCameraPosition(rotz * camMf * fMo);
864  }
865 
866  if (I.display != NULL)
868  }
869 
870  if (displayObject)
871  display_scene(w44o, this->scene, I, curColor);
872 
873  if (displayCamera)
874  display_scene(w44c, camera, I, camColor);
875 
877  vpImagePoint iP;
878  vpImagePoint iP_1;
879  poseList.push_back(cMo);
880  fMoList.push_back(fMo);
881 
882  int iter = 0;
883 
884  if (changed || extCamChanged) {
885  cameraTrajectory.clear();
886  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
887  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
888 
889  while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
890  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
891  cameraTrajectory.push_back(iP);
892  if (camTrajType == CT_LINE) {
893  if (iter != 0)
895  } else if (camTrajType == CT_POINT)
897  ++iter_poseList;
898  ++iter_fMoList;
899  iter++;
900  iP_1 = iP;
901  }
902  extCamChanged = false;
903  } else {
904  iP = projectCameraTrajectory(I, cMo, fMo);
905  cameraTrajectory.push_back(iP);
906 
907  for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
908  if (camTrajType == CT_LINE) {
909  if (iter != 0)
911  } else if (camTrajType == CT_POINT)
913  iter++;
914  iP_1 = *it;
915  }
916  }
917 
918  if (poseList.size() > nbrPtLimit) {
919  poseList.pop_front();
920  }
921  if (fMoList.size() > nbrPtLimit) {
922  fMoList.pop_front();
923  }
924  if (cameraTrajectory.size() > nbrPtLimit) {
925  cameraTrajectory.pop_front();
926  }
927  }
928 }
929 
942 {
943  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
944 
945  vpHomogeneousMatrix camMft = rotz * cam_Mf;
946 
947  double u;
948  double v;
949  // if(px_ext != 1 && py_ext != 1)
950  // we assume px_ext and py_ext > 0
951  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
952  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
953  u = (double)I.getWidth() / (2 * px_ext);
954  v = (double)I.getHeight() / (2 * py_ext);
955  } else {
956  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
957  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
958  }
959 
960  vp2jlc_matrix(camMft.inverse(), w44cext);
961  vp2jlc_matrix(fMo * cMo.inverse(), w44c);
962  vp2jlc_matrix(fMo, w44o);
963 
964  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
965  x = w44cext[2][0] + w44cext[3][0];
966  y = w44cext[2][1] + w44cext[3][1];
967  z = w44cext[2][2] + w44cext[3][2];
968  add_vwstack("start", "vrp", x, y, z);
969  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
970  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
971  add_vwstack("start", "window", -u, u, -v, v);
972 
973  if (displayImageSimulator) {
974  I = 255;
975 
976  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
977  vpImageSimulator *imSim = &(*it);
978  imSim->setCameraPosition(rotz * cam_Mf * fMo);
980  }
981 
982  if (I.display != NULL)
984  }
985 
986  if (displayObject)
987  display_scene(w44o, this->scene, I, curColor);
988  if (displayCamera)
989  display_scene(w44c, camera, I, camColor);
990 }
991 
1001 {
1002  if (!sceneInitialized)
1003  throw(vpException(vpException::notInitialized, "The scene has to be initialized"));
1004 
1005  double u;
1006  double v;
1007  // if(px_int != 1 && py_int != 1)
1008  // we assume px_int and py_int > 0
1009  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
1010  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
1011  u = (double)I.getWidth() / (2 * px_int);
1012  v = (double)I.getHeight() / (2 * py_int);
1013  } else {
1014  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1015  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1016  }
1017 
1018  float o44c[4][4], o44cd[4][4], x, y, z;
1019  Matrix id = IDENTITY_MATRIX;
1020 
1021  vp2jlc_matrix(cMo.inverse(), o44c);
1022  vp2jlc_matrix(cdMo.inverse(), o44cd);
1023 
1024  if (displayImageSimulator) {
1025  I = 255;
1026 
1027  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
1028  vpImageSimulator *imSim = &(*it);
1029  imSim->setCameraPosition(rotz * camMf * fMo);
1030  imSim->getImage(I, getInternalCameraParameters(I));
1031  }
1032 
1033  if (I.display != NULL)
1034  vpDisplay::display(I);
1035  }
1036 
1037  add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
1038  x = o44c[2][0] + o44c[3][0];
1039  y = o44c[2][1] + o44c[3][1];
1040  z = o44c[2][2] + o44c[3][2];
1041  add_vwstack("start", "vrp", x, y, z);
1042  add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
1043  add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
1044  add_vwstack("start", "window", -u, u, -v, v);
1045  if (displayObject)
1046  display_scene(id, this->scene, I, curColor);
1047 
1048  add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
1049  x = o44cd[2][0] + o44cd[3][0];
1050  y = o44cd[2][1] + o44cd[3][1];
1051  z = o44cd[2][2] + o44cd[3][2];
1052  add_vwstack("start", "vrp", x, y, z);
1053  add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
1054  add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
1055  add_vwstack("start", "window", -u, u, -v, v);
1056  if (displayDesiredObject) {
1057  if (desiredObject == D_TOOL)
1059  else
1061  }
1062 }
1063 
1075 {
1076  bool changed = false;
1077  vpHomogeneousMatrix displacement = navigation(I, changed);
1078 
1079  // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
1080  // 0*/)
1081  if (std::fabs(displacement[2][3]) >
1082  std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
1083  camMf2 = camMf2 * displacement;
1084 
1085  f2Mf = camMf2.inverse() * camMf;
1086 
1087  camMf = camMf2 * displacement * f2Mf;
1088 
1089  double u;
1090  double v;
1091  // if(px_ext != 1 && py_ext != 1)
1092  // we assume px_ext and py_ext > 0
1093  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
1094  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
1095  u = (double)I.getWidth() / (2 * px_ext);
1096  v = (double)I.getHeight() / (2 * py_ext);
1097  } else {
1098  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1099  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1100  }
1101 
1102  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1103 
1104  vp2jlc_matrix(camMf.inverse(), w44cext);
1105  vp2jlc_matrix(fMc, w44c);
1106  vp2jlc_matrix(fMo, w44o);
1107 
1108  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
1109  x = w44cext[2][0] + w44cext[3][0];
1110  y = w44cext[2][1] + w44cext[3][1];
1111  z = w44cext[2][2] + w44cext[3][2];
1112  add_vwstack("start", "vrp", x, y, z);
1113  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
1114  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
1115  add_vwstack("start", "window", -u, u, -v, v);
1116  if ((object == CUBE) || (object == SPHERE)) {
1117  add_vwstack("start", "type", PERSPECTIVE);
1118  }
1119 
1120  if (displayImageSimulator) {
1121  I = 255;
1122  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
1123  vpImageSimulator *imSim = &(*it);
1124  imSim->setCameraPosition(rotz * camMf * fMo);
1125  imSim->getImage(I, getInternalCameraParameters(I));
1126  }
1127  if (I.display != NULL)
1128  vpDisplay::display(I);
1129  }
1130 
1131  if (displayObject)
1132  display_scene(w44o, this->scene, I, curColor);
1133 
1134  if (displayCamera)
1135  display_scene(w44c, camera, I, camColor);
1136 
1138  vpImagePoint iP;
1139  vpImagePoint iP_1;
1140  poseList.push_back(cMo);
1141  fMoList.push_back(fMo);
1142 
1143  int iter = 0;
1144 
1145  if (changed || extCamChanged) {
1146  cameraTrajectory.clear();
1147  std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
1148  std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
1149 
1150  while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
1151  iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
1152  cameraTrajectory.push_back(iP);
1153  // vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
1154  if (camTrajType == CT_LINE) {
1155  if (iter != 0)
1157  } else if (camTrajType == CT_POINT)
1159  ++iter_poseList;
1160  ++iter_fMoList;
1161  iter++;
1162  iP_1 = iP;
1163  }
1164  extCamChanged = false;
1165  } else {
1166  iP = projectCameraTrajectory(I, cMo, fMo);
1167  cameraTrajectory.push_back(iP);
1168 
1169  for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
1170  if (camTrajType == CT_LINE) {
1171  if (iter != 0)
1173  } else if (camTrajType == CT_POINT)
1175  iter++;
1176  iP_1 = *it;
1177  }
1178  }
1179 
1180  if (poseList.size() > nbrPtLimit) {
1181  poseList.pop_front();
1182  }
1183  if (fMoList.size() > nbrPtLimit) {
1184  fMoList.pop_front();
1185  }
1186  if (cameraTrajectory.size() > nbrPtLimit) {
1187  cameraTrajectory.pop_front();
1188  }
1189  }
1190 }
1191 
1204 {
1205  float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1206 
1207  vpHomogeneousMatrix camMft = rotz * cam_Mf;
1208 
1209  double u;
1210  double v;
1211  // if(px_ext != 1 && py_ext != 1)
1212  // we assume px_ext and py_ext > 0
1213  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
1214  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
1215  u = (double)I.getWidth() / (2 * px_ext);
1216  v = (double)I.getHeight() / (2 * py_ext);
1217  } else {
1218  u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1219  v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
1220  }
1221 
1222  vp2jlc_matrix(camMft.inverse(), w44cext);
1223  vp2jlc_matrix(fMo * cMo.inverse(), w44c);
1224  vp2jlc_matrix(fMo, w44o);
1225 
1226  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
1227  x = w44cext[2][0] + w44cext[3][0];
1228  y = w44cext[2][1] + w44cext[3][1];
1229  z = w44cext[2][2] + w44cext[3][2];
1230  add_vwstack("start", "vrp", x, y, z);
1231  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
1232  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
1233  add_vwstack("start", "window", -u, u, -v, v);
1234 
1235  if (displayImageSimulator) {
1236  I = 255;
1237  for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
1238  vpImageSimulator *imSim = &(*it);
1239  imSim->setCameraPosition(rotz * cam_Mf * fMo);
1240  imSim->getImage(I, getInternalCameraParameters(I));
1241  }
1242  if (I.display != NULL)
1243  vpDisplay::display(I);
1244  }
1245 
1246  if (displayObject)
1247  display_scene(w44o, this->scene, I, curColor);
1248  if (displayCamera)
1249  display_scene(w44c, camera, I, camColor);
1250 }
1251 
1270  const std::list<vpHomogeneousMatrix> &list_cMo,
1271  const std::list<vpHomogeneousMatrix> &list_fMo,
1272  const vpHomogeneousMatrix &cMf)
1273 {
1274  if (list_cMo.size() != list_fMo.size())
1275  throw(vpException(vpException::dimensionError, "The two lists must have the same size"));
1276 
1277  vpImagePoint iP;
1278  vpImagePoint iP_1;
1279  int iter = 0;
1280 
1281  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1282  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1283 
1284  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1285  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1286  if (camTrajType == CT_LINE) {
1287  if (iter != 0)
1289  } else if (camTrajType == CT_POINT)
1291  ++it_cMo;
1292  ++it_fMo;
1293  iter++;
1294  iP_1 = iP;
1295  }
1296 }
1297 
1315 void vpWireFrameSimulator::displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
1316  const std::list<vpHomogeneousMatrix> &list_fMo,
1317  const vpHomogeneousMatrix &cMf)
1318 {
1319  if (list_cMo.size() != list_fMo.size())
1320  throw(vpException(vpException::dimensionError, "The two lists must have the same size"));
1321 
1322  vpImagePoint iP;
1323  vpImagePoint iP_1;
1324  int iter = 0;
1325 
1326  std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1327  std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1328 
1329  while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1330  iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
1331  if (camTrajType == CT_LINE) {
1332  if (iter != 0)
1334  } else if (camTrajType == CT_POINT)
1336  ++it_cMo;
1337  ++it_fMo;
1338  iter++;
1339  iP_1 = iP;
1340  }
1341 }
1342 
1347 {
1348  double width = vpMath::minimum(I.getWidth(), I.getHeight());
1349  vpImagePoint iP;
1350  vpImagePoint trash;
1351  bool clicked = false;
1352  bool clickedUp = false;
1354 
1355  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
1356  changed = false;
1357 
1358  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1359 
1360  if (!blocked)
1361  clicked = vpDisplay::getClick(I, trash, b, false);
1362 
1363  if (blocked)
1364  clickedUp = vpDisplay::getClickUp(I, trash, b, false);
1365 
1366  if (clicked) {
1367  if (b == vpMouseButton::button1)
1368  blockedr = true;
1369  if (b == vpMouseButton::button2)
1370  blockedz = true;
1371  if (b == vpMouseButton::button3)
1372  blockedt = true;
1373  blocked = true;
1374  }
1375  if (clickedUp) {
1376  if (b == vpMouseButton::button1) {
1377  old_iPr = vpImagePoint(-1, -1);
1378  blockedr = false;
1379  }
1380  if (b == vpMouseButton::button2) {
1381  old_iPz = vpImagePoint(-1, -1);
1382  blockedz = false;
1383  }
1384  if (b == vpMouseButton::button3) {
1385  old_iPt = vpImagePoint(-1, -1);
1386  blockedt = false;
1387  }
1388  if (!(blockedr || blockedz || blockedt)) {
1389  blocked = false;
1390  while (vpDisplay::getClick(I, trash, b, false)) {
1391  };
1392  }
1393  }
1394 
1396 
1397  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
1398  double diffi = iP.get_i() - old_iPr.get_i();
1399  double diffj = iP.get_j() - old_iPr.get_j();
1400  // cout << "delta :" << diffj << endl;;
1401  double anglei = diffi * 360 / width;
1402  double anglej = diffj * 360 / width;
1403  mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
1404  changed = true;
1405  }
1406 
1407  if (blockedr)
1408  old_iPr = iP;
1409 
1410  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
1411  double diffi = iP.get_i() - old_iPz.get_i();
1412  mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1413  changed = true;
1414  }
1415 
1416  if (blockedz)
1417  old_iPz = iP;
1418 
1419  if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
1420  double diffi = iP.get_i() - old_iPt.get_i();
1421  double diffj = iP.get_j() - old_iPt.get_j();
1422  mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
1423  changed = true;
1424  }
1425 
1426  if (blockedt)
1427  old_iPt = iP;
1428 
1429  return mov;
1430 }
1431 
1436 {
1437  double width = vpMath::minimum(I.getWidth(), I.getHeight());
1438  vpImagePoint iP;
1439  vpImagePoint trash;
1440  bool clicked = false;
1441  bool clickedUp = false;
1443 
1444  vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
1445  changed = false;
1446 
1447  // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
1448 
1449  if (!blocked)
1450  clicked = vpDisplay::getClick(I, trash, b, false);
1451 
1452  if (blocked)
1453  clickedUp = vpDisplay::getClickUp(I, trash, b, false);
1454 
1455  if (clicked) {
1456  if (b == vpMouseButton::button1)
1457  blockedr = true;
1458  if (b == vpMouseButton::button2)
1459  blockedz = true;
1460  if (b == vpMouseButton::button3)
1461  blockedt = true;
1462  blocked = true;
1463  }
1464  if (clickedUp) {
1465  if (b == vpMouseButton::button1) {
1466  old_iPr = vpImagePoint(-1, -1);
1467  blockedr = false;
1468  }
1469  if (b == vpMouseButton::button2) {
1470  old_iPz = vpImagePoint(-1, -1);
1471  blockedz = false;
1472  }
1473  if (b == vpMouseButton::button3) {
1474  old_iPt = vpImagePoint(-1, -1);
1475  blockedt = false;
1476  }
1477  if (!(blockedr || blockedz || blockedt)) {
1478  blocked = false;
1479  while (vpDisplay::getClick(I, trash, b, false)) {
1480  };
1481  }
1482  }
1483 
1485 
1486  // std::cout << "point : " << iP << std::endl;
1487 
1488  if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
1489  double diffi = iP.get_i() - old_iPr.get_i();
1490  double diffj = iP.get_j() - old_iPr.get_j();
1491  // cout << "delta :" << diffj << endl;;
1492  double anglei = diffi * 360 / width;
1493  double anglej = diffj * 360 / width;
1494  mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
1495  changed = true;
1496  }
1497 
1498  if (blockedr)
1499  old_iPr = iP;
1500 
1501  if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
1502  double diffi = iP.get_i() - old_iPz.get_i();
1503  mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1504  changed = true;
1505  }
1506 
1507  if (blockedz)
1508  old_iPz = iP;
1509 
1510  if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
1511  double diffi = iP.get_i() - old_iPt.get_i();
1512  double diffj = iP.get_j() - old_iPt.get_j();
1513  mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
1514  changed = true;
1515  }
1516 
1517  if (blockedt)
1518  old_iPt = iP;
1519 
1520  return mov;
1521 }
1522 
1527  const vpHomogeneousMatrix &fMo_)
1528 {
1529  vpPoint point;
1530  point.setWorldCoordinates(0, 0, 0);
1531 
1532  point.track(rotz * (camMf * fMo_ * cMo_.inverse()));
1533 
1534  vpImagePoint iP;
1535 
1537 
1538  return iP;
1539 }
1540 
1545  const vpHomogeneousMatrix &cMo_,
1546  const vpHomogeneousMatrix &fMo_)
1547 {
1548  vpPoint point;
1549  point.setWorldCoordinates(0, 0, 0);
1550 
1551  point.track(rotz * (camMf * fMo_ * cMo_.inverse()));
1552 
1553  vpImagePoint iP;
1554 
1556 
1557  return iP;
1558 }
1559 
1564  const vpHomogeneousMatrix &fMo_,
1565  const vpHomogeneousMatrix &cMf)
1566 {
1567  vpPoint point;
1568  point.setWorldCoordinates(0, 0, 0);
1569 
1570  point.track(rotz * (cMf * fMo_ * cMo_.inverse()));
1571 
1572  vpImagePoint iP;
1573 
1575 
1576  return iP;
1577 }
1578 
1583  const vpHomogeneousMatrix &cMo_,
1584  const vpHomogeneousMatrix &fMo_,
1585  const vpHomogeneousMatrix &cMf)
1586 {
1587  vpPoint point;
1588  point.setWorldCoordinates(0, 0, 0);
1589 
1590  point.track(rotz * (cMf * fMo_ * cMo_.inverse()));
1591 
1592  vpImagePoint iP;
1593 
1595 
1596  return iP;
1597 }
vpDisplay * display
Definition: vpImage.h:135
A cylinder of 80cm length and 10cm radius.
static bool getPointerPosition(const vpImage< unsigned char > &I, vpImagePoint &ip)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:484
A tire represented by 2 circles with radius 10cm and 15cm.
double get_i() const
Definition: vpImagePoint.h:204
unsigned int getWidth() const
Definition: vpImage.h:237
Three parallel lines with equation y=-5, y=0, y=5.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
A cylindrical tool is attached to the camera.
static std::string getenv(const char *env)
Definition: vpIoTools.cpp:379
static bool getClickUp(const vpImage< unsigned char > &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)
std::list< vpImageSimulator > objectImage
error that can be emited by ViSP classes.
Definition: vpException.h:71
std::list< vpImagePoint > cameraTrajectory
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix cdMo
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:431
vpHomogeneousMatrix fMo
double get_j() const
Definition: vpImagePoint.h:215
std::list< vpHomogeneousMatrix > fMoList
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
void setCameraPosition(const vpHomogeneousMatrix &cMt)
The object displayed at the desired position is a circle.
vpHomogeneousMatrix f2Mf
static void display(const vpImage< unsigned char > &I)
Memory allocation error.
Definition: vpException.h:88
Class which enables to project an image in the 3D space and get the view of a virtual camera...
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
vpHomogeneousMatrix cMo
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:429
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1806
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
void getExternalImage(vpImage< unsigned char > &I)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
static double rad(double deg)
Definition: vpMath.h:108
void displayTrajectory(const vpImage< unsigned char > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
vpCameraTrajectoryDisplayType camTrajType
vpSceneDesiredObject desiredObject
void getInternalImage(vpImage< unsigned char > &I)
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
Three parallel lines representing a road.
vpHomogeneousMatrix fMc
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
vpImagePoint projectCameraTrajectory(const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
vpHomogeneousMatrix inverse() const
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
vpHomogeneousMatrix rotz
vpHomogeneousMatrix refMo
unsigned int getHeight() const
Definition: vpImage.h:179
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
std::list< vpHomogeneousMatrix > poseList
vpHomogeneousMatrix camMf2