Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
tutorial-pose-from-points-live.cpp
1 
2 #include <iostream>
3 
4 #include <visp3/core/vpConfig.h>
5 
7 // Comment / uncomment following lines to use the specific 3rd party compatible with your camera
8 // #undef VISP_HAVE_V4L2
9 // #undef VISP_HAVE_DC1394
10 // #undef VISP_HAVE_CMU1394
11 // #undef VISP_HAVE_FLYCAPTURE
12 // #undef VISP_HAVE_REALSENSE2
13 // #undef HAVE_OPENCV_HIGHGUI
14 // #undef HAVE_OPENCV_VIDEOIO
16 
17 #if defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_PUGIXML) && \
18  (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \
19  defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)) || \
20  ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI))|| ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_VIDEOIO))
21 
22 #ifdef VISP_HAVE_MODULE_SENSOR
23 #include <visp3/sensor/vp1394CMUGrabber.h>
24 #include <visp3/sensor/vp1394TwoGrabber.h>
25 #include <visp3/sensor/vpFlyCaptureGrabber.h>
26 #include <visp3/sensor/vpRealSense2.h>
27 #include <visp3/sensor/vpV4l2Grabber.h>
28 #endif
29 #include <visp3/core/vpXmlParserCamera.h>
30 #include <visp3/gui/vpDisplayFactory.h>
31 
32 #if (VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI)
33 #include <opencv2/highgui/highgui.hpp> // for cv::VideoCapture
34 #elif (VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_VIDEOIO)
35 #include <opencv2/videoio/videoio.hpp> // for cv::VideoCapture
36 #endif
37 
38 #include "pose_helper.h"
39 
40 int main(int argc, char **argv)
41 {
42 #ifdef ENABLE_VISP_NAMESPACE
43  using namespace VISP_NAMESPACE_NAME;
44 #endif
45 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
46  std::shared_ptr<vpDisplay> display;
47 #else
48  vpDisplay *display = nullptr;
49 #endif
50  try {
51  std::string opt_intrinsic_file; // xml file obtained from camera calibration
52  std::string opt_camera_name; // corresponding camera name in the xml calibration file
53  double opt_square_width = 0.12;
54  int opt_device = 0; // For OpenCV and V4l2 grabber to set the camera device
55 
56  for (int i = 1; i < argc; i++) {
57  if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
58  opt_intrinsic_file = std::string(argv[++i]);
59  }
60  else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) {
61  opt_camera_name = std::string(argv[++i]);
62  }
63  else if (std::string(argv[i]) == "--camera-device" && i + 1 < argc) {
64  opt_device = atoi(argv[++i]);
65  }
66  else if (std::string(argv[i]) == "--square-width" && i + 1 < argc) {
67  opt_device = atoi(argv[++i]);
68  }
69  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
70  std::cout << "\nUsage: " << argv[0]
71  << " [--camera-device <camera device> (default: 0)]"
72  << " [--intrinsic <xml calibration file> (default: empty)]"
73  << " [--camera-name <camera name in xml calibration file> (default: empty)]"
74  << " [--square-width <square width in meter (default: 0.12)]"
75  << " [--help] [-h]\n"
76  << std::endl
77  << "Example using default camera parameters and square size:\n"
78  << " " << argv[0] << "\n"
79  << std::endl
80  << "Example fully tuned for a 0.1m x 0.1m square:\n"
81  << " " << argv[0] << " --intrinsic camera.xml --camera-name Camera --square-width 0.1\n"
82  << std::endl;
83  return EXIT_SUCCESS;
84  }
85  }
86 
89 
91 #if defined(VISP_HAVE_V4L2)
92  vpV4l2Grabber g;
93  std::ostringstream device;
94  device << "/dev/video" << opt_device;
95  std::cout << "Use Video 4 Linux grabber on device " << device.str() << std::endl;
96  g.setDevice(device.str());
97  g.setScale(1);
98  g.open(I);
99  cam.initPersProjWithoutDistortion(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
100 #elif defined(VISP_HAVE_DC1394)
101  (void)opt_device; // To avoid non used warning
102  std::cout << "Use DC1394 grabber" << std::endl;
104  g.open(I);
105  cam.initPersProjWithoutDistortion(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
106 #elif defined(VISP_HAVE_CMU1394)
107  (void)opt_device; // To avoid non used warning
108  std::cout << "Use CMU1394 grabber" << std::endl;
110  g.open(I);
111  cam.initPersProjWithoutDistortion(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
112 #elif defined(VISP_HAVE_FLYCAPTURE)
113  (void)opt_device; // To avoid non used warning
114  std::cout << "Use FlyCapture grabber" << std::endl;
116  g.open(I);
117  cam.initPersProjWithoutDistortion(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
118 #elif defined(VISP_HAVE_REALSENSE2)
119  (void)opt_device; // To avoid non used warning
120  std::cout << "Use Realsense 2 grabber" << std::endl;
121  vpRealSense2 g;
122  rs2::config config;
123  config.disable_stream(RS2_STREAM_DEPTH);
124  config.disable_stream(RS2_STREAM_INFRARED);
125  config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
126  g.open(config);
127  g.acquire(I);
128 
129  std::cout << "Read camera parameters from Realsense device" << std::endl;
131 #elif ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI))|| ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_VIDEOIO))
132  std::cout << "Use OpenCV grabber on device " << opt_device << std::endl;
133  cv::VideoCapture g(opt_device); // Open the default camera
134  if (!g.isOpened()) { // Check if we succeeded
135  std::cout << "Failed to open the camera" << std::endl;
136  return EXIT_FAILURE;
137  }
138  cv::Mat frame;
139  g >> frame; // get a new frame from camera
140  vpImageConvert::convert(frame, I);
141  cam.initPersProjWithoutDistortion(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
142 #endif
144 
145  // Parameters of our camera
146  vpXmlParserCamera parser;
147  if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
148  std::cout << "Intrinsic file: " << opt_intrinsic_file << std::endl;
149  std::cout << "Camera name : " << opt_camera_name << std::endl;
150  if (parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithDistortion) ==
152  std::cout << "Succeed to read camera parameters from xml file" << std::endl;
153  }
154  else {
155  std::cout << "Unable to read camera parameters from xml file" << std::endl;
156  }
157  }
158 
159  std::cout << "Square width : " << opt_square_width << std::endl;
160  std::cout << cam << std::endl;
161 
162  // The pose container
164 
165  std::vector<vpDot2> dot(4);
166  std::vector<vpPoint> point; // 3D coordinates of the points
167  std::vector<vpImagePoint> ip; // 2D coordinates of the points in pixels
168  double L = opt_square_width / 2.;
169  point.push_back(vpPoint(-L, -L, 0));
170  point.push_back(vpPoint(L, -L, 0));
171  point.push_back(vpPoint(L, L, 0));
172  point.push_back(vpPoint(-L, L, 0));
173 
174 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
175  display = vpDisplayFactory::createDisplay(I);
176 #else
178 #endif
179 
180  bool quit = false;
181  bool apply_cv = false; // apply computer vision
182  bool init_cv = true; // initialize tracking and pose computation
183 
184  while (!quit) {
185  double t_begin = vpTime::measureTimeMs();
186  // Image Acquisition
187 #if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || \
188  defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)
189  g.acquire(I);
190 #elif ((VISP_HAVE_OPENCV_VERSION < 0x030000) && defined(HAVE_OPENCV_HIGHGUI))|| ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_VIDEOIO))
191  g >> frame;
192  vpImageConvert::convert(frame, I);
193 #endif
195  if (apply_cv) {
196  try {
197  ip = track(I, dot, init_cv);
198  computePose(point, ip, cam, init_cv, cMo);
199  vpDisplay::displayFrame(I, cMo, cam, opt_square_width, vpColor::none, 3);
200  if (init_cv)
201  init_cv = false; // turn off the computer vision initialisation specific stuff
202 
203  { // Display estimated pose in [m] and [deg]
204  vpPoseVector pose(cMo);
205  std::stringstream ss;
206  ss << "Translation: " << std::setprecision(5) << pose[0] << " " << pose[1] << " " << pose[2] << " [m]";
207  vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
208  ss.str(""); // erase ss
209  ss << "Rotation tu: " << std::setprecision(4) << vpMath::deg(pose[3]) << " " << vpMath::deg(pose[4]) << " "
210  << vpMath::deg(pose[5]) << " [deg]";
211  vpDisplay::displayText(I, 80, 20, ss.str(), vpColor::red);
212  }
213  }
214  catch (...) {
215  std::cout << "Computer vision failure." << std::endl;
216  apply_cv = false;
217  init_cv = true;
218  }
219  }
220  vpDisplay::displayText(I, 20, 20, "Right click: quit", vpColor::red);
221  if (apply_cv) {
222  vpDisplay::displayText(I, 40, 20, "Computer vision in progress...", vpColor::red);
223  }
224  else {
225  vpDisplay::displayText(I, 40, 20, "Left click : start", vpColor::red);
226  }
228  if (vpDisplay::getClick(I, button, false)) {
229  if (button == vpMouseButton::button3) {
230  quit = true;
231  }
232  else if (button == vpMouseButton::button1) {
233  apply_cv = true;
234  }
235  }
236  {
237  std::stringstream ss;
238  ss << "Time: " << vpTime::measureTimeMs() - t_begin << " ms";
239  vpDisplay::displayText(I, 20, I.getWidth() - 100, ss.str(), vpColor::red);
240  }
241  vpDisplay::flush(I);
242  }
243  }
244  catch (const vpException &e) {
245  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
246  }
247 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
248  if (display != nullptr) {
249  delete display;
250  }
251 #endif
252 }
253 
254 #else
255 
256 int main()
257 {
258  std::cout << "There are missing 3rd parties to run this tutorial" << std::endl;
259 }
260 
261 #endif
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.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition: vpColor.h:198
static const vpColor none
Definition: vpColor.h:210
Class that defines generic functionalities for display.
Definition: vpDisplay.h:178
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:60
const char * getMessage() const
Definition: vpException.cpp:65
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:242
unsigned int getHeight() const
Definition: vpImage.h:181
static double deg(double rad)
Definition: vpMath.h:119
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
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)
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()