Visual Servoing Platform  version 3.4.0
tutorial-pose-from-points-realsense-T265.cpp
1 
2 #include <visp3/core/vpConfig.h>
3 #ifdef VISP_HAVE_MODULE_SENSOR
4 #include <visp3/sensor/vpRealSense2.h>
5 #endif
6 #include <visp3/gui/vpDisplayGDI.h>
7 #include <visp3/gui/vpDisplayOpenCV.h>
8 #include <visp3/gui/vpDisplayX.h>
9 
10 #include "pose_helper.h"
11 
12 int main(int argc, char **argv)
13 {
14 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
15  defined(VISP_HAVE_REALSENSE2) && \
16  (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
17  try {
18  double opt_square_width = 0.12;
19  int opt_camera_index = 1; // camera index: 1. Left, 2.Right
20 
21  for (int i = 0; i < argc; i++) {
22  if (std::string(argv[i]) == "--camera_index" && i + 1 < argc) {
23  opt_camera_index = atoi(argv[i + 1]);
24  } else if(std::string(argv[i]) == "--square_width" && i + 1 < argc) {
25  opt_square_width = atoi(argv[i + 1]);
26  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
27  std::cout << "\nUsage: " << argv[0]
28  << " [--camera_index <1.Left | 2.Right> (default: 1)]"
29  << " [--square_width <square width in meter (default: 0.12)] [--help] [-h]\n"
30  << "\nExample using right camera and square size 0.1:\n"
31  << " " << argv[0] << "--camera_index 2 --square_width 0.1\n"
32  << std::endl;
33  return 0;
34  }
35  }
36 
38 
40  std::cout << "Use Realsense 2 grabber" << std::endl;
41  vpRealSense2 g;
42  rs2::config config;
43  config.enable_stream(RS2_STREAM_FISHEYE, 1);
44  config.enable_stream(RS2_STREAM_FISHEYE, 2);
45 
46  g.open(config);
47  if(opt_camera_index == 1) // Left camera
48  g.acquire(&I, NULL, NULL);
49  else
50  g.acquire(NULL, &I, NULL);
51 
52  std::cout << "Read camera parameters from Realsense device" << std::endl;
53  // Parameters of our camera
56 
57  std::cout << "Square width : " << opt_square_width << std::endl;
58  std::cout << cam << std::endl;
59 
60  // The pose container
62 
63  std::vector<vpDot2> dot(4);
64  std::vector<vpPoint> point; // 3D coordinates of the points
65  std::vector<vpImagePoint> ip; // 2D coordinates of the points in pixels
66  double L = opt_square_width / 2.;
67  point.push_back(vpPoint(-L, -L, 0));
68  point.push_back(vpPoint( L, -L, 0));
69  point.push_back(vpPoint( L, L, 0));
70  point.push_back(vpPoint(-L, L, 0));
71 
72 #if defined(VISP_HAVE_X11)
73  vpDisplayX d(I);
74 #elif defined(VISP_HAVE_GDI)
75  vpDisplayGDI d(I);
76 #elif defined(VISP_HAVE_OPENCV)
77  vpDisplayOpenCV d(I);
78 #endif
79 
80  bool quit = false;
81  bool apply_cv = false; // apply computer vision
82  bool init_cv = true; // initialize tracking and pose computation
83 
84  while (! quit) {
85  double t_begin = vpTime::measureTimeMs();
86 
87  if(opt_camera_index == 1)
88  g.acquire(&I, NULL, NULL);
89  else
90  g.acquire(NULL, &I, NULL);
91 
93  if (apply_cv) {
94  try {
95  ip = track(I, dot, init_cv);
96  computePose(point, ip, cam, init_cv, cMo);
97  vpDisplay::displayFrame(I, cMo, cam, opt_square_width, vpColor::none, 3);
98  if (init_cv)
99  init_cv = false; // turn off the computer vision initialisation specific stuff
100 
101  { // Display estimated pose in [m] and [deg]
102  vpPoseVector pose(cMo);
103  std::stringstream ss;
104  ss << "Translation: " << std::setprecision(5) << pose[0] << " " << pose[1] << " " << pose[2] << " [m]";
105  vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
106  ss.str(""); // erase ss
107  ss << "Rotation tu: " << std::setprecision(4) << vpMath::deg(pose[3]) << " " << vpMath::deg(pose[4]) << " " << vpMath::deg(pose[5]) << " [deg]";
108  vpDisplay::displayText(I, 80, 20, ss.str(), vpColor::red);
109  }
110  }
111  catch(...) {
112  std::cout << "Computer vision failure." << std::endl;
113  apply_cv = false;
114  init_cv = true;
115  }
116  }
117  vpDisplay::displayText(I, 20, 20, "Right click: quit", vpColor::red);
118  if (apply_cv) {
119  vpDisplay::displayText(I, 40, 20, "Computer vision in progress...", vpColor::red);
120  } else {
121  vpDisplay::displayText(I, 40, 20, "Left click : start", vpColor::red);
122  }
124  if (vpDisplay::getClick(I, button, false)) {
125  if (button == vpMouseButton::button3) {
126  quit = true;
127  }
128  else if (button == vpMouseButton::button1) {
129  apply_cv = true;
130  }
131  }
132  {
133  std::stringstream ss;
134  ss << "Time: " << vpTime::measureTimeMs() - t_begin << " ms";
135  vpDisplay::displayText(I, 20, I.getWidth()-100, ss.str(), vpColor::red);
136  }
137  vpDisplay::flush(I);
138  }
139  } catch (const vpException &e) {
140  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
141  }
142 #elif !defined(VISP_HAVE_REALSENSE2)
143  (void)argc; (void)argv;
144  std::cout << "You do not realsense2 SDK functionality enabled..." << std::endl;
145  std::cout << "Tip:" << std::endl;
146  std::cout << "- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
147  return EXIT_SUCCESS;
148 #elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
149  (void)argc; (void)argv;
150  std::cout << "You do not build ViSP with c++11 or higher compiler flag" << std::endl;
151  std::cout << "Tip:" << std::endl;
152  std::cout << "- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
153 #elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
154  (void)argc; (void)argv;
155  std::cout << "Install a 3rd party dedicated to image display (X11, GDI, OpenCV), configure and build ViSP again to use this example" << std::endl;
156 #elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
157  (void)argc; (void)argv;
158  std::cout << "Install librealsense version > 2.31.0" << std::endl;
159 #endif
160 }
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
unsigned int getWidth() const
Definition: vpImage.h:246
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:71
static void flush(const vpImage< unsigned char > &I)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
static double deg(double rad)
Definition: vpMath.h:103
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
const char * getMessage() const
Definition: vpException.cpp:90
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)