Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
tutorial-pose-from-points-live.cpp
1 
2 #include <visp3/core/vpConfig.h>
3 #ifdef VISP_HAVE_MODULE_SENSOR
4 #include <visp3/sensor/vp1394CMUGrabber.h>
5 #include <visp3/sensor/vp1394TwoGrabber.h>
6 #include <visp3/sensor/vpFlyCaptureGrabber.h>
7 #include <visp3/sensor/vpRealSense2.h>
8 #include <visp3/sensor/vpV4l2Grabber.h>
9 #endif
10 #include <visp3/core/vpXmlParserCamera.h>
11 #include <visp3/gui/vpDisplayGDI.h>
12 #include <visp3/gui/vpDisplayOpenCV.h>
13 #include <visp3/gui/vpDisplayX.h>
14 
15 #if defined(HAVE_OPENCV_VIDEOIO)
16 #include <opencv2/videoio.hpp>
17 #endif
18 
19 #include "pose_helper.h"
20 
21 // Comment / uncomment following lines to use the specific 3rd party compatible with your camera
23 // #undef VISP_HAVE_V4L2
24 // #undef VISP_HAVE_DC1394
25 // #undef VISP_HAVE_CMU1394
26 // #undef VISP_HAVE_FLYCAPTURE
27 // #undef VISP_HAVE_REALSENSE2
28 // #undef HAVE_OPENCV_VIDEOIO
30 
31 int main(int argc, char **argv)
32 {
33 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && defined(VISP_HAVE_PUGIXML) && \
34  (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \
35  defined(HAVE_OPENCV_VIDEOIO) || defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2))
36  try {
37  std::string opt_intrinsic_file; // xml file obtained from camera calibration
38  std::string opt_camera_name; // corresponding camera name in the xml calibration file
39  double opt_square_width = 0.12;
40  int opt_device = 0; // For OpenCV and V4l2 grabber to set the camera device
41 
42  for (int i = 0; i < argc; i++) {
43  if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
44  opt_intrinsic_file = std::string(argv[i + 1]);
45  }
46  else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
47  opt_camera_name = std::string(argv[i + 1]);
48  }
49  else if (std::string(argv[i]) == "--camera_device" && i + 1 < argc) {
50  opt_device = atoi(argv[i + 1]);
51  }
52  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
53  std::cout << "\nUsage: " << argv[0] << " [--camera_device <camera device> (default: 0)]"
54  << " [--intrinsic <xml calibration file> (default: empty)]"
55  " [--camera_name <camera name in xml calibration file> (default: empty)]"
56  " [--square_width <square width in meter (default: 0.12)] [--help] [-h]\n"
57  << "\nExample using default camera parameters and square size:\n"
58  << " " << argv[0] << "\n"
59  << "\nExample fully tuned for a 0.1m x 0.1m square:\n"
60  << " " << argv[0] << " --intrinsic camera.xml --camera_name Camera --square_width 0.1\n"
61  << std::endl;
62  return EXIT_SUCCESS;
63  }
64  }
65 
67 
68  // Parameters of our camera
69  vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
70  vpXmlParserCamera parser;
71  if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
72  std::cout << "Intrinsic file: " << opt_intrinsic_file << std::endl;
73  std::cout << "Camera name : " << opt_camera_name << std::endl;
74  if (parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithDistortion) ==
76  std::cout << "Succeed to read camera parameters from xml file" << std::endl;
77  }
78  else {
79  std::cout << "Unable to read camera parameters from xml file" << std::endl;
80  }
81  }
82 
84 #if defined(VISP_HAVE_V4L2)
85  vpV4l2Grabber g;
86  std::ostringstream device;
87  device << "/dev/video" << opt_device;
88  std::cout << "Use Video 4 Linux grabber on device " << device.str() << std::endl;
89  g.setDevice(device.str());
90  g.setScale(1);
91  g.open(I);
92 #elif defined(VISP_HAVE_DC1394)
93  (void)opt_device; // To avoid non used warning
94  std::cout << "Use DC1394 grabber" << std::endl;
96  g.open(I);
97 #elif defined(VISP_HAVE_CMU1394)
98  (void)opt_device; // To avoid non used warning
99  std::cout << "Use CMU1394 grabber" << std::endl;
101  g.open(I);
102 #elif defined(VISP_HAVE_FLYCAPTURE)
103  (void)opt_device; // To avoid non used warning
104  std::cout << "Use FlyCapture grabber" << std::endl;
106  g.open(I);
107 #elif defined(VISP_HAVE_REALSENSE2)
108  (void)opt_device; // To avoid non used warning
109  std::cout << "Use Realsense 2 grabber" << std::endl;
110  vpRealSense2 g;
111  rs2::config config;
112  config.disable_stream(RS2_STREAM_DEPTH);
113  config.disable_stream(RS2_STREAM_INFRARED);
114  config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
115  g.open(config);
116  g.acquire(I);
117 
118  std::cout << "Read camera parameters from Realsense device" << std::endl;
120 #elif defined(HAVE_OPENCV_VIDEOIO)
121  std::cout << "Use OpenCV grabber on device " << opt_device << std::endl;
122  cv::VideoCapture g(opt_device); // Open the default camera
123  if (!g.isOpened()) { // Check if we succeeded
124  std::cout << "Failed to open the camera" << std::endl;
125  return EXIT_FAILURE;
126  }
127  cv::Mat frame;
128  g >> frame; // get a new frame from camera
129  vpImageConvert::convert(frame, I);
130 #endif
132 
133  std::cout << "Square width : " << opt_square_width << std::endl;
134  std::cout << cam << std::endl;
135 
136  // The pose container
138 
139  std::vector<vpDot2> dot(4);
140  std::vector<vpPoint> point; // 3D coordinates of the points
141  std::vector<vpImagePoint> ip; // 2D coordinates of the points in pixels
142  double L = opt_square_width / 2.;
143  point.push_back(vpPoint(-L, -L, 0));
144  point.push_back(vpPoint(L, -L, 0));
145  point.push_back(vpPoint(L, L, 0));
146  point.push_back(vpPoint(-L, L, 0));
147 
148 #if defined(VISP_HAVE_X11)
149  vpDisplayX d(I);
150 #elif defined(VISP_HAVE_GDI)
151  vpDisplayGDI d(I);
152 #elif defined(HAVE_OPENCV_HIGHGUI)
153  vpDisplayOpenCV d(I);
154 #endif
155 
156  bool quit = false;
157  bool apply_cv = false; // apply computer vision
158  bool init_cv = true; // initialize tracking and pose computation
159 
160  while (!quit) {
161  double t_begin = vpTime::measureTimeMs();
162  // Image Acquisition
163 #if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \
164  defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)
165  g.acquire(I);
166 #elif defined(HAVE_OPENCV_VIDEOIO)
167  g >> frame;
168  vpImageConvert::convert(frame, I);
169 #endif
171  if (apply_cv) {
172  try {
173  ip = track(I, dot, init_cv);
174  computePose(point, ip, cam, init_cv, cMo);
175  vpDisplay::displayFrame(I, cMo, cam, opt_square_width, vpColor::none, 3);
176  if (init_cv)
177  init_cv = false; // turn off the computer vision initialisation specific stuff
178 
179  { // Display estimated pose in [m] and [deg]
180  vpPoseVector pose(cMo);
181  std::stringstream ss;
182  ss << "Translation: " << std::setprecision(5) << pose[0] << " " << pose[1] << " " << pose[2] << " [m]";
183  vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
184  ss.str(""); // erase ss
185  ss << "Rotation tu: " << std::setprecision(4) << vpMath::deg(pose[3]) << " " << vpMath::deg(pose[4]) << " "
186  << vpMath::deg(pose[5]) << " [deg]";
187  vpDisplay::displayText(I, 80, 20, ss.str(), vpColor::red);
188  }
189  }
190  catch (...) {
191  std::cout << "Computer vision failure." << std::endl;
192  apply_cv = false;
193  init_cv = true;
194  }
195  }
196  vpDisplay::displayText(I, 20, 20, "Right click: quit", vpColor::red);
197  if (apply_cv) {
198  vpDisplay::displayText(I, 40, 20, "Computer vision in progress...", vpColor::red);
199  }
200  else {
201  vpDisplay::displayText(I, 40, 20, "Left click : start", vpColor::red);
202  }
204  if (vpDisplay::getClick(I, button, false)) {
205  if (button == vpMouseButton::button3) {
206  quit = true;
207  }
208  else if (button == vpMouseButton::button1) {
209  apply_cv = true;
210  }
211  }
212  {
213  std::stringstream ss;
214  ss << "Time: " << vpTime::measureTimeMs() - t_begin << " ms";
215  vpDisplay::displayText(I, 20, I.getWidth() - 100, ss.str(), vpColor::red);
216  }
217  vpDisplay::flush(I);
218  }
219  }
220  catch (const vpException &e) {
221  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
222  }
223 #elif (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
224  (void)argc;
225  (void)argv;
226  std::cout << "Install a 3rd party dedicated to frame grabbing (dc1394, cmu1394, v4l2, OpenCV, FlyCapture, "
227  "Realsense2), configure and build ViSP again to use this example"
228  << std::endl;
229 #else
230  (void)argc;
231  (void)argv;
232  std::cout << "Install a 3rd party dedicated to image display (X11, GDI, OpenCV), configure and build ViSP again to "
233  "use this example"
234  << std::endl;
235 #endif
236 }
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void open(vpImage< unsigned char > &I)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void open(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
const char * getMessage() const
Definition: vpException.cpp:64
void open(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static double deg(double rad)
Definition: vpMath.h:117
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)
VISP_EXPORT double measureTimeMs()