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),
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);
bool displayImageSimulator
A cylinder of 80cm length and 10cm radius.
static bool getPointerPosition(const vpImage< unsigned char > &I, vpImagePoint &ip)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
A tire represented by 2 circles with radius 10cm and 15cm.
unsigned int getWidth() const
Three parallel lines with equation y=-5, y=0, y=5.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class to define colors available for display functionnalities.
A cylindrical tool is attached to the camera.
static bool getClickUp(const vpImage< unsigned char > &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true)
std::list< vpImageSimulator > objectImage
error that can be emited by ViSP classes.
std::list< vpImagePoint > cameraTrajectory
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.
virtual ~vpWireFrameSimulator()
std::list< vpHomogeneousMatrix > fMoList
Class that defines what is a point.
static Type maximum(const Type &a, const Type &b)
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
void setCameraPosition(const vpHomogeneousMatrix &cMt)
The object displayed at the desired position is a circle.
static void display(const vpImage< unsigned char > &I)
Class which enables to project an image in the 3D space and get the view of a virtual camera...
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
bool displayDesiredObject
double get_x() const
Get the point x coordinate in the image plane.
static Type minimum(const Type &a, const Type &b)
void getExternalImage(vpImage< unsigned char > &I)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
static double rad(double deg)
void displayTrajectory(const vpImage< unsigned char > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
vpCameraTrajectoryDisplayType camTrajType
vpSceneDesiredObject desiredObject
void getInternalImage(vpImage< unsigned char > &I)
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
bool displayCameraTrajectory
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
Three parallel lines representing a road.
void setWorldCoordinates(const double oX, const double oY, const double oZ)
vpImagePoint projectCameraTrajectory(const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
vpHomogeneousMatrix inverse() const
Used to indicate that a parameter is not initialized.
vpHomogeneousMatrix refMo
unsigned int getHeight() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
std::list< vpHomogeneousMatrix > poseList
vpHomogeneousMatrix camMf2