Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
31int main(int argc, char **argv)
32{
33#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
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)
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 }
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).
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:132
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
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double deg(double rad)
Definition vpMath.h:106
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.
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
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()