45 #include <visp3/robot/vpWireFrameSimulator.h>
49 #include "vpClipping.h"
50 #include "vpCoreDisplay.h"
51 #include "vpKeyword.h"
55 #include "vpProjection.h"
56 #include "vpRfstack.h"
58 #include "vpTmstack.h"
61 #include "vpVwstack.h"
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>
69 extern Point2i *point2i;
70 extern Point2i *listpoint2i;
80 Byte b = (Byte)*get_rfstack();
84 memmove((
char *)m, (
char *)mat,
sizeof(Matrix));
85 View_to_Matrix(get_vwstack(), *(get_tmstack()));
86 postmult_matrix(m, *(get_tmstack()));
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;
94 set_Bound_face_display(clip, b);
96 point_3D_2D(clip->point.ptr, clip->point.nbr, (
int)I.
getWidth(), (
int)I.
getHeight(), point2i);
97 for (; fp < fend; fp++) {
99 wireframe_Face(fp, point2i);
100 Point2i *pt = listpoint2i;
101 for (
int i = 1; i < fp->vertex.nbr; i++) {
106 if (fp->vertex.nbr > 2) {
126 Byte b = (Byte)*get_rfstack();
130 memmove((
char *)m, (
char *)mat,
sizeof(Matrix));
131 View_to_Matrix(get_vwstack(), *(get_tmstack()));
132 postmult_matrix(m, *(get_tmstack()));
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;
140 set_Bound_face_display(clip, b);
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++) {
152 if (fp->vertex.nbr > 2) {
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()
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++)
187 scene_dir = scene_dirs[i];
188 sceneDirExists =
true;
191 if (!sceneDirExists) {
194 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
196 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
210 scene.bound.ptr = NULL;
229 free_Bound_scene(&(this->
scene));
231 free_Bound_scene(&(this->
camera));
260 char name_cam[FILENAME_MAX];
261 char name[FILENAME_MAX];
266 const char *scene_dir_ = scene_dir.c_str();
267 if (strlen(scene_dir_) >= FILENAME_MAX) {
271 strcpy(name_cam, scene_dir_);
273 strcat(name_cam,
"/camera.bnd");
276 strcat(name_cam,
"/tool.bnd");
277 set_scene(name_cam, &(this->
camera), 1.0);
280 strcpy(name, scene_dir_);
283 strcat(name,
"/3pts.bnd");
287 strcat(name,
"/cube.bnd");
291 strcat(name,
"/plate.bnd");
295 strcat(name,
"/plate_6cm.bnd");
299 strcat(name,
"/rectangle.bnd");
303 strcat(name,
"/square10cm.bnd");
307 strcat(name,
"/diamond.bnd");
311 strcat(name,
"/trapezoid.bnd");
315 strcat(name,
"/line.bnd");
319 strcat(name,
"/road.bnd");
323 strcat(name,
"/circles2.bnd");
327 strcat(name,
"/pipe.bnd");
331 strcat(name,
"/circle.bnd");
335 strcat(name,
"/sphere.bnd");
339 strcat(name,
"/cylinder.bnd");
343 strcat(name,
"/plan.bnd");
347 strcat(name,
"/point_cloud.bnd");
351 set_scene(name, &(this->
scene), 1.0);
353 scene_dir_ = scene_dir.c_str();
354 if (strlen(scene_dir_) >= FILENAME_MAX) {
363 strcpy(name, scene_dir_);
364 strcat(name,
"/circle_sq2.bnd");
368 strcpy(name, scene_dir_);
369 strcat(name,
"/tool.bnd");
376 load_rfstack(IS_INSIDE);
378 add_rfstack(IS_BACK);
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);
411 const std::list<vpImageSimulator> &imObj)
431 char name_cam[FILENAME_MAX];
432 char name[FILENAME_MAX];
437 const char *scene_dir_ = scene_dir.c_str();
438 if (strlen(scene_dir_) >= FILENAME_MAX) {
442 strcpy(name_cam, scene_dir_);
443 strcat(name_cam,
"/camera.bnd");
446 if (strlen(obj) >= FILENAME_MAX) {
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) {
461 if (strlen(desired_object) >= FILENAME_MAX) {
465 strcpy(name, desired_object);
466 model = getExtension(desired_object);
467 if (model == BND_MODEL)
469 else if (model == WRL_MODEL)
471 else if (model == UNKNOWN_MODEL) {
475 add_rfstack(IS_BACK);
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);
504 const std::list<vpImageSimulator> &imObj)
525 char name_cam[FILENAME_MAX];
526 char name[FILENAME_MAX];
530 const char *scene_dir_ = scene_dir.c_str();
531 if (strlen(scene_dir_) >= FILENAME_MAX) {
535 strcpy(name_cam, scene_dir_);
536 strcat(name_cam,
"/camera.bnd");
539 strcpy(name, scene_dir_);
542 strcat(name,
"/3pts.bnd");
546 strcat(name,
"/cube.bnd");
550 strcat(name,
"/plate.bnd");
554 strcat(name,
"/plate_6cm.bnd");
558 strcat(name,
"/rectangle.bnd");
562 strcat(name,
"/square10cm.bnd");
566 strcat(name,
"/diamond.bnd");
570 strcat(name,
"/trapezoid.bnd");
574 strcat(name,
"/line.bnd");
578 strcat(name,
"/road.bnd");
582 strcat(name,
"/circles2.bnd");
586 strcat(name,
"/pipe.bnd");
590 strcat(name,
"/circle.bnd");
594 strcat(name,
"/sphere.bnd");
598 strcat(name,
"/cylinder.bnd");
602 strcat(name,
"/plan.bnd");
606 strcat(name,
"/point_cloud.bnd");
610 set_scene(name, &(this->
scene), 1.0);
613 load_rfstack(IS_INSIDE);
615 add_rfstack(IS_BACK);
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);
664 char name_cam[FILENAME_MAX];
665 char name[FILENAME_MAX];
669 const char *scene_dir_ = scene_dir.c_str();
670 if (strlen(scene_dir_) >= FILENAME_MAX) {
674 strcpy(name_cam, scene_dir_);
675 strcat(name_cam,
"/camera.bnd");
678 if (strlen(obj) >= FILENAME_MAX) {
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) {
693 add_rfstack(IS_BACK);
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);
752 float o44c[4][4], o44cd[4][4], x, y, z;
753 Matrix
id = IDENTITY_MATRIX;
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);
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);
810 bool changed =
false;
815 if (std::fabs(displacement[2][3]) >
816 std::numeric_limits<double>::epsilon() )
836 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
839 vp2jlc_matrix(
fMc, w44c);
840 vp2jlc_matrix(
fMo, w44o);
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);
851 add_vwstack(
"start",
"type", PERSPECTIVE);
883 std::list<vpHomogeneousMatrix>::const_iterator iter_poseList =
poseList.begin();
884 std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList =
fMoList.begin();
886 while ((iter_poseList !=
poseList.end()) && (iter_fMoList !=
fMoList.end())) {
940 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
957 vp2jlc_matrix(camMft.
inverse(), w44cext);
959 vp2jlc_matrix(
fMo, w44o);
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);
1015 float o44c[4][4], o44cd[4][4], x, y, z;
1016 Matrix
id = IDENTITY_MATRIX;
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);
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);
1073 bool changed =
false;
1078 if (std::fabs(displacement[2][3]) >
1079 std::numeric_limits<double>::epsilon() )
1099 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1102 vp2jlc_matrix(
fMc, w44c);
1103 vp2jlc_matrix(
fMo, w44o);
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);
1144 std::list<vpHomogeneousMatrix>::const_iterator iter_poseList =
poseList.begin();
1145 std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList =
fMoList.begin();
1147 while ((iter_poseList !=
poseList.end()) && (iter_fMoList !=
fMoList.end())) {
1202 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1219 vp2jlc_matrix(camMft.
inverse(), w44cext);
1221 vp2jlc_matrix(
fMo, w44o);
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);
1267 const std::list<vpHomogeneousMatrix> &list_cMo,
1268 const std::list<vpHomogeneousMatrix> &list_fMo,
1271 if (list_cMo.size() != list_fMo.size())
1278 std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1279 std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1281 while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1313 const std::list<vpHomogeneousMatrix> &list_fMo,
1316 if (list_cMo.size() != list_fMo.size())
1323 std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1324 std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1326 while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1348 bool clicked =
false;
1349 bool clickedUp =
false;
1398 double anglei = diffi * 360 / width;
1399 double anglej = diffj * 360 / width;
1409 mov.
buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1419 mov.
buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
1437 bool clicked =
false;
1438 bool clickedUp =
false;
1489 double anglei = diffi * 360 / width;
1490 double anglej = diffj * 360 / width;
1500 mov.
buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1510 mov.
buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
Class to define RGB colors available for display functionalities.
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.
@ notInitialized
Used to indicate that a parameter is not initialized.
@ dimensionError
Bad dimension.
@ memoryAllocationError
Memory allocation error.
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 ...
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
unsigned int getHeight() const
static double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static Type minimum(const Type &a, const Type &b)
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 ...
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
void setWorldCoordinates(double oX, double oY, double oZ)
vpSceneDesiredObject desiredObject
vpHomogeneousMatrix camMf2
@ 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
bool displayCameraTrajectory
void getInternalImage(vpImage< unsigned char > &I)
bool displayImageSimulator
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 camMf
std::list< vpImagePoint > cameraTrajectory
bool displayDesiredObject
std::list< vpHomogeneousMatrix > fMoList
virtual ~vpWireFrameSimulator()
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.
void getExternalImage(vpImage< unsigned char > &I)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
std::list< vpHomogeneousMatrix > poseList