Visual Servoing Platform version 3.5.0
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
72extern Point2i *point2i;
73extern Point2i *listpoint2i;
74
75/*
76 Copy the scene corresponding to the registeresd parameters in the image.
77*/
78void 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*/
122void 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),
177 desiredObject(D_STANDARD), camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
178 desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true), cameraTrajectory(), poseList(),
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),
181 displayDesiredObject(false), displayCamera(false), displayImageSimulator(false), cameraFactor(1.),
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 initialize the camera name"));
272 }
273
274 strcpy(name_cam, scene_dir_);
275 if (desiredObject != D_TOOL) {
276 strcat(name_cam, "/camera.bnd");
277 set_scene(name_cam, &camera, cameraFactor);
278 } else {
279 strcat(name_cam, "/tool.bnd");
280 set_scene(name_cam, &(this->camera), 1.0);
281 }
282
283 strcpy(name, scene_dir_);
284 switch (obj) {
285 case THREE_PTS: {
286 strcat(name, "/3pts.bnd");
287 break;
288 }
289 case CUBE: {
290 strcat(name, "/cube.bnd");
291 break;
292 }
293 case PLATE: {
294 strcat(name, "/plate.bnd");
295 break;
296 }
297 case SMALL_PLATE: {
298 strcat(name, "/plate_6cm.bnd");
299 break;
300 }
301 case RECTANGLE: {
302 strcat(name, "/rectangle.bnd");
303 break;
304 }
305 case SQUARE_10CM: {
306 strcat(name, "/square10cm.bnd");
307 break;
308 }
309 case DIAMOND: {
310 strcat(name, "/diamond.bnd");
311 break;
312 }
313 case TRAPEZOID: {
314 strcat(name, "/trapezoid.bnd");
315 break;
316 }
317 case THREE_LINES: {
318 strcat(name, "/line.bnd");
319 break;
320 }
321 case ROAD: {
322 strcat(name, "/road.bnd");
323 break;
324 }
325 case TIRE: {
326 strcat(name, "/circles2.bnd");
327 break;
328 }
329 case PIPE: {
330 strcat(name, "/pipe.bnd");
331 break;
332 }
333 case CIRCLE: {
334 strcat(name, "/circle.bnd");
335 break;
336 }
337 case SPHERE: {
338 strcat(name, "/sphere.bnd");
339 break;
340 }
341 case CYLINDER: {
342 strcat(name, "/cylinder.bnd");
343 break;
344 }
345 case PLAN: {
346 strcat(name, "/plan.bnd");
347 break;
348 }
349 case POINT_CLOUD: {
350 strcat(name, "/point_cloud.bnd");
351 break;
352 }
353 }
354 set_scene(name, &(this->scene), 1.0);
355
356 scene_dir_ = scene_dir.c_str();
357 if (strlen(scene_dir_) >= FILENAME_MAX) {
358 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the desired object name"));
359 }
360
361 switch (desiredObject) {
362 case D_STANDARD: {
363 break;
364 }
365 case D_CIRCLE: {
366 strcpy(name, scene_dir_);
367 strcat(name, "/circle_sq2.bnd");
368 break;
369 }
370 case D_TOOL: {
371 strcpy(name, scene_dir_);
372 strcat(name, "/tool.bnd");
373 break;
374 }
375 }
376 set_scene(name, &(this->desiredScene), 1.0);
377
378 if (obj == PIPE)
379 load_rfstack(IS_INSIDE);
380 else
381 add_rfstack(IS_BACK);
382
383 add_vwstack("start", "depth", 0.0, 100.0);
384 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
385 add_vwstack("start", "type", PERSPECTIVE);
386
387 sceneInitialized = true;
388 displayObject = true;
390 displayCamera = true;
392}
393
414 const std::list<vpImageSimulator> &imObj)
415{
416 initScene(obj, desired_object);
417 objectImage = imObj;
419}
420
432void 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;
439
440 const char *scene_dir_ = scene_dir.c_str();
441 if (strlen(scene_dir_) >= FILENAME_MAX) {
442 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
443 }
444
445 strcpy(name_cam, scene_dir_);
446 strcat(name_cam, "/camera.bnd");
447 set_scene(name_cam, &camera, cameraFactor);
448
449 if (strlen(obj) >= FILENAME_MAX) {
450 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the name"));
451 }
452
453 strcpy(name, obj);
454 Model_3D model;
455 model = getExtension(obj);
456 if (model == BND_MODEL)
457 set_scene(name, &(this->scene), 1.0);
458 else if (model == WRL_MODEL)
459 set_scene_wrl(name, &(this->scene), 1.0);
460 else if (model == UNKNOWN_MODEL) {
461 vpERROR_TRACE("Unknown file extension for the 3D model");
462 }
463
464 if (strlen(desired_object) >= FILENAME_MAX) {
465 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
466 }
467
468 strcpy(name, desired_object);
469 model = getExtension(desired_object);
470 if (model == BND_MODEL)
471 set_scene(name, &(this->desiredScene), 1.0);
472 else if (model == WRL_MODEL)
473 set_scene_wrl(name, &(this->desiredScene), 1.0);
474 else if (model == UNKNOWN_MODEL) {
475 vpERROR_TRACE("Unknown file extension for the 3D model");
476 }
477
478 add_rfstack(IS_BACK);
479
480 add_vwstack("start", "depth", 0.0, 100.0);
481 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
482 add_vwstack("start", "type", PERSPECTIVE);
483
484 sceneInitialized = true;
485 displayObject = true;
487 displayCamera = true;
488}
489
506void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object,
507 const std::list<vpImageSimulator> &imObj)
508{
509 initScene(obj, desired_object);
510 objectImage = imObj;
512}
513
527{
528 char name_cam[FILENAME_MAX];
529 char name[FILENAME_MAX];
530
531 object = obj;
532
533 const char *scene_dir_ = scene_dir.c_str();
534 if (strlen(scene_dir_) >= FILENAME_MAX) {
535 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
536 }
537
538 strcpy(name_cam, scene_dir_);
539 strcat(name_cam, "/camera.bnd");
540 set_scene(name_cam, &camera, cameraFactor);
541
542 strcpy(name, scene_dir_);
543 switch (obj) {
544 case THREE_PTS: {
545 strcat(name, "/3pts.bnd");
546 break;
547 }
548 case CUBE: {
549 strcat(name, "/cube.bnd");
550 break;
551 }
552 case PLATE: {
553 strcat(name, "/plate.bnd");
554 break;
555 }
556 case SMALL_PLATE: {
557 strcat(name, "/plate_6cm.bnd");
558 break;
559 }
560 case RECTANGLE: {
561 strcat(name, "/rectangle.bnd");
562 break;
563 }
564 case SQUARE_10CM: {
565 strcat(name, "/square10cm.bnd");
566 break;
567 }
568 case DIAMOND: {
569 strcat(name, "/diamond.bnd");
570 break;
571 }
572 case TRAPEZOID: {
573 strcat(name, "/trapezoid.bnd");
574 break;
575 }
576 case THREE_LINES: {
577 strcat(name, "/line.bnd");
578 break;
579 }
580 case ROAD: {
581 strcat(name, "/road.bnd");
582 break;
583 }
584 case TIRE: {
585 strcat(name, "/circles2.bnd");
586 break;
587 }
588 case PIPE: {
589 strcat(name, "/pipe.bnd");
590 break;
591 }
592 case CIRCLE: {
593 strcat(name, "/circle.bnd");
594 break;
595 }
596 case SPHERE: {
597 strcat(name, "/sphere.bnd");
598 break;
599 }
600 case CYLINDER: {
601 strcat(name, "/cylinder.bnd");
602 break;
603 }
604 case PLAN: {
605 strcat(name, "/plan.bnd");
606 break;
607 }
608 case POINT_CLOUD: {
609 strcat(name, "/point_cloud.bnd");
610 break;
611 }
612 }
613 set_scene(name, &(this->scene), 1.0);
614
615 if (obj == PIPE)
616 load_rfstack(IS_INSIDE);
617 else
618 add_rfstack(IS_BACK);
619
620 add_vwstack("start", "depth", 0.0, 100.0);
621 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
622 add_vwstack("start", "type", PERSPECTIVE);
623
624 sceneInitialized = true;
625 displayObject = true;
626 displayCamera = true;
627
628 displayDesiredObject = false;
629 displayImageSimulator = false;
630}
631
648void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
649{
650 initScene(obj);
651 objectImage = imObj;
653}
654
666{
667 char name_cam[FILENAME_MAX];
668 char name[FILENAME_MAX];
669
670 object = THREE_PTS;
671
672 const char *scene_dir_ = scene_dir.c_str();
673 if (strlen(scene_dir_) >= FILENAME_MAX) {
674 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the camera name"));
675 }
676
677 strcpy(name_cam, scene_dir_);
678 strcat(name_cam, "/camera.bnd");
679 set_scene(name_cam, &camera, cameraFactor);
680
681 if (strlen(obj) >= FILENAME_MAX) {
682 throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the name"));
683 }
684
685 strcpy(name, obj);
686 Model_3D model;
687 model = getExtension(obj);
688 if (model == BND_MODEL)
689 set_scene(name, &(this->scene), 1.0);
690 else if (model == WRL_MODEL)
691 set_scene_wrl(name, &(this->scene), 1.0);
692 else if (model == UNKNOWN_MODEL) {
693 vpERROR_TRACE("Unknown file extension for the 3D model");
694 }
695
696 add_rfstack(IS_BACK);
697
698 add_vwstack("start", "depth", 0.0, 100.0);
699 add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
700 add_vwstack("start", "type", PERSPECTIVE);
701
702 sceneInitialized = true;
703 displayObject = true;
704 displayCamera = true;
705}
706
722void vpWireFrameSimulator::initScene(const char *obj, const std::list<vpImageSimulator> &imObj)
723{
724 initScene(obj);
725 objectImage = imObj;
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
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);
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
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 {
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
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
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);
1031 }
1032
1033 if (I.display != NULL)
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);
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
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);
1126 }
1127 if (I.display != NULL)
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 {
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
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);
1241 }
1242 if (I.display != NULL)
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
1315void 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}
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static bool getClickUp(const vpImage< unsigned char > &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static bool getPointerPosition(const vpImage< unsigned char > &I, vpImagePoint &ip)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition: vpException.h:98
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ memoryAllocationError
Memory allocation error.
Definition: vpException.h:88
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:88
double get_j() const
Definition: vpImagePoint.h:214
double get_i() const
Definition: vpImagePoint.h:203
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
vpDisplay * display
Definition: vpImage.h:144
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:353
static double rad(double deg)
Definition: vpMath.h:110
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
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:82
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
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:393