48 #include <visp3/robot/vpWireFrameSimulator.h>
52 #include "vpClipping.h"
53 #include "vpCoreDisplay.h"
54 #include "vpKeyword.h"
58 #include "vpProjection.h"
59 #include "vpRfstack.h"
61 #include "vpTmstack.h"
64 #include "vpVwstack.h"
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>
72 extern Point2i *point2i;
73 extern Point2i *listpoint2i;
83 Byte b = (Byte)*get_rfstack();
87 memmove((
char *)m, (
char *)mat,
sizeof(Matrix));
88 View_to_Matrix(get_vwstack(), *(get_tmstack()));
89 postmult_matrix(m, *(get_tmstack()));
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;
97 set_Bound_face_display(clip, b);
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++) {
109 if (fp->vertex.nbr > 2) {
129 Byte b = (Byte)*get_rfstack();
133 memmove((
char *)m, (
char *)mat,
sizeof(Matrix));
134 View_to_Matrix(get_vwstack(), *(get_tmstack()));
135 postmult_matrix(m, *(get_tmstack()));
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;
143 set_Bound_face_display(clip, b);
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++) {
155 if (fp->vertex.nbr > 2) {
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()
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++)
190 scene_dir = scene_dirs[i];
191 sceneDirExists =
true;
194 if (!sceneDirExists) {
197 std::cout <<
"The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
199 std::cout <<
"Cannot get VISP_SCENES_DIR environment variable" << std::endl;
213 scene.bound.ptr = NULL;
232 free_Bound_scene(&(this->
scene));
234 free_Bound_scene(&(this->
camera));
263 char name_cam[FILENAME_MAX];
264 char name[FILENAME_MAX];
269 const char *scene_dir_ = scene_dir.c_str();
270 if (strlen(scene_dir_) >= FILENAME_MAX) {
274 strcpy(name_cam, scene_dir_);
276 strcat(name_cam,
"/camera.bnd");
279 strcat(name_cam,
"/tool.bnd");
280 set_scene(name_cam, &(this->
camera), 1.0);
283 strcpy(name, scene_dir_);
286 strcat(name,
"/3pts.bnd");
290 strcat(name,
"/cube.bnd");
294 strcat(name,
"/plate.bnd");
298 strcat(name,
"/plate_6cm.bnd");
302 strcat(name,
"/rectangle.bnd");
306 strcat(name,
"/square10cm.bnd");
310 strcat(name,
"/diamond.bnd");
314 strcat(name,
"/trapezoid.bnd");
318 strcat(name,
"/line.bnd");
322 strcat(name,
"/road.bnd");
326 strcat(name,
"/circles2.bnd");
330 strcat(name,
"/pipe.bnd");
334 strcat(name,
"/circle.bnd");
338 strcat(name,
"/sphere.bnd");
342 strcat(name,
"/cylinder.bnd");
346 strcat(name,
"/plan.bnd");
350 strcat(name,
"/point_cloud.bnd");
354 set_scene(name, &(this->
scene), 1.0);
356 scene_dir_ = scene_dir.c_str();
357 if (strlen(scene_dir_) >= FILENAME_MAX) {
366 strcpy(name, scene_dir_);
367 strcat(name,
"/circle_sq2.bnd");
371 strcpy(name, scene_dir_);
372 strcat(name,
"/tool.bnd");
379 load_rfstack(IS_INSIDE);
381 add_rfstack(IS_BACK);
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);
414 const std::list<vpImageSimulator> &imObj)
434 char name_cam[FILENAME_MAX];
435 char name[FILENAME_MAX];
440 const char *scene_dir_ = scene_dir.c_str();
441 if (strlen(scene_dir_) >= FILENAME_MAX) {
445 strcpy(name_cam, scene_dir_);
446 strcat(name_cam,
"/camera.bnd");
449 if (strlen(obj) >= FILENAME_MAX) {
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) {
464 if (strlen(desired_object) >= FILENAME_MAX) {
468 strcpy(name, desired_object);
469 model = getExtension(desired_object);
470 if (model == BND_MODEL)
472 else if (model == WRL_MODEL)
474 else if (model == UNKNOWN_MODEL) {
478 add_rfstack(IS_BACK);
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);
507 const std::list<vpImageSimulator> &imObj)
528 char name_cam[FILENAME_MAX];
529 char name[FILENAME_MAX];
533 const char *scene_dir_ = scene_dir.c_str();
534 if (strlen(scene_dir_) >= FILENAME_MAX) {
538 strcpy(name_cam, scene_dir_);
539 strcat(name_cam,
"/camera.bnd");
542 strcpy(name, scene_dir_);
545 strcat(name,
"/3pts.bnd");
549 strcat(name,
"/cube.bnd");
553 strcat(name,
"/plate.bnd");
557 strcat(name,
"/plate_6cm.bnd");
561 strcat(name,
"/rectangle.bnd");
565 strcat(name,
"/square10cm.bnd");
569 strcat(name,
"/diamond.bnd");
573 strcat(name,
"/trapezoid.bnd");
577 strcat(name,
"/line.bnd");
581 strcat(name,
"/road.bnd");
585 strcat(name,
"/circles2.bnd");
589 strcat(name,
"/pipe.bnd");
593 strcat(name,
"/circle.bnd");
597 strcat(name,
"/sphere.bnd");
601 strcat(name,
"/cylinder.bnd");
605 strcat(name,
"/plan.bnd");
609 strcat(name,
"/point_cloud.bnd");
613 set_scene(name, &(this->
scene), 1.0);
616 load_rfstack(IS_INSIDE);
618 add_rfstack(IS_BACK);
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);
667 char name_cam[FILENAME_MAX];
668 char name[FILENAME_MAX];
672 const char *scene_dir_ = scene_dir.c_str();
673 if (strlen(scene_dir_) >= FILENAME_MAX) {
677 strcpy(name_cam, scene_dir_);
678 strcat(name_cam,
"/camera.bnd");
681 if (strlen(obj) >= FILENAME_MAX) {
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) {
696 add_rfstack(IS_BACK);
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);
755 float o44c[4][4], o44cd[4][4], x, y, z;
756 Matrix
id = IDENTITY_MATRIX;
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);
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);
813 bool changed =
false;
818 if (std::fabs(displacement[2][3]) >
819 std::numeric_limits<double>::epsilon() )
839 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
842 vp2jlc_matrix(
fMc, w44c);
843 vp2jlc_matrix(
fMo, w44o);
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);
854 add_vwstack(
"start",
"type", PERSPECTIVE);
886 std::list<vpHomogeneousMatrix>::const_iterator iter_poseList =
poseList.begin();
887 std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList =
fMoList.begin();
889 while ((iter_poseList !=
poseList.end()) && (iter_fMoList !=
fMoList.end())) {
943 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
960 vp2jlc_matrix(camMft.
inverse(), w44cext);
962 vp2jlc_matrix(
fMo, w44o);
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);
1018 float o44c[4][4], o44cd[4][4], x, y, z;
1019 Matrix
id = IDENTITY_MATRIX;
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);
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);
1076 bool changed =
false;
1081 if (std::fabs(displacement[2][3]) >
1082 std::numeric_limits<double>::epsilon() )
1102 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1105 vp2jlc_matrix(
fMc, w44c);
1106 vp2jlc_matrix(
fMo, w44o);
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);
1147 std::list<vpHomogeneousMatrix>::const_iterator iter_poseList =
poseList.begin();
1148 std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList =
fMoList.begin();
1150 while ((iter_poseList !=
poseList.end()) && (iter_fMoList !=
fMoList.end())) {
1205 float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
1222 vp2jlc_matrix(camMft.
inverse(), w44cext);
1224 vp2jlc_matrix(
fMo, w44o);
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);
1270 const std::list<vpHomogeneousMatrix> &list_cMo,
1271 const std::list<vpHomogeneousMatrix> &list_fMo,
1274 if (list_cMo.size() != list_fMo.size())
1281 std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1282 std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1284 while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1316 const std::list<vpHomogeneousMatrix> &list_fMo,
1319 if (list_cMo.size() != list_fMo.size())
1326 std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
1327 std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
1329 while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
1351 bool clicked =
false;
1352 bool clickedUp =
false;
1401 double anglei = diffi * 360 / width;
1402 double anglej = diffj * 360 / width;
1412 mov.
buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1422 mov.
buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
1440 bool clicked =
false;
1441 bool clickedUp =
false;
1492 double anglei = diffi * 360 / width;
1493 double anglej = diffj * 360 / width;
1503 mov.
buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
1513 mov.
buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
Class to define RGB colors available for display functionnalities.
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.
@ 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