Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
69extern Point2i *point2i;
70extern Point2i *listpoint2i;
71
72/*
73 Copy the scene corresponding to the registeresd parameters in the image.
74*/
75void 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*/
119void 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;
387 displayCamera = true;
389}
390
411 const std::list<vpImageSimulator> &imObj)
412{
413 initScene(obj, desired_object);
414 objectImage = imObj;
416}
417
429void 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;
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;
484 displayCamera = true;
485}
486
503void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object,
504 const std::list<vpImageSimulator> &imObj)
505{
506 initScene(obj, desired_object);
507 objectImage = imObj;
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
645void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
646{
647 initScene(obj);
648 objectImage = imObj;
650}
651
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
719void vpWireFrameSimulator::initScene(const char *obj, const std::list<vpImageSimulator> &imObj)
720{
721 initScene(obj);
722 objectImage = imObj;
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
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);
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
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 {
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
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
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);
1028 }
1029
1030 if (I.display != NULL)
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);
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
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);
1123 }
1124 if (I.display != NULL)
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 {
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
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);
1238 }
1239 if (I.display != NULL)
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
1312void 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 ...
double get_j() const
double get_i() const
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
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)
static bool checkDirectory(const std::string &dirname)
static std::string getenv(const std::string &env)
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
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:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
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:388