Visual Servoing Platform version 3.5.0
tutorial-pose-from-points-live.cpp
1
2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpV4l2Grabber.h>
5#include <visp3/sensor/vp1394CMUGrabber.h>
6#include <visp3/sensor/vp1394TwoGrabber.h>
7#include <visp3/sensor/vpFlyCaptureGrabber.h>
8#include <visp3/sensor/vpRealSense2.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#include "pose_helper.h"
16
17// Comment / uncomment following lines to use the specific 3rd party compatible with your camera
19//#undef VISP_HAVE_V4L2
20//#undef VISP_HAVE_DC1394
21//#undef VISP_HAVE_CMU1394
22//#undef VISP_HAVE_FLYCAPTURE
23//#undef VISP_HAVE_REALSENSE2
24//#undef VISP_HAVE_OPENCV
26
27int main(int argc, char **argv)
28{
29#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
30 (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2) )
31 try {
32 std::string opt_intrinsic_file; // xml file obtained from camera calibration
33 std::string opt_camera_name; // corresponding camera name in the xml calibration file
34 double opt_square_width = 0.12;
35 int opt_device = 0; // For OpenCV and V4l2 grabber to set the camera device
36
37 for (int i = 0; i < argc; i++) {
38 if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
39 opt_intrinsic_file = std::string(argv[i + 1]);
40 } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
41 opt_camera_name = std::string(argv[i + 1]);
42 } else if (std::string(argv[i]) == "--camera_device" && i + 1 < argc) {
43 opt_device = atoi(argv[i + 1]);
44 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
45 std::cout << "\nUsage: " << argv[0]
46 << " [--camera_device <camera device> (default: 0)]"
47 << " [--intrinsic <xml calibration file> (default: empty)]"
48 " [--camera_name <camera name in xml calibration file> (default: empty)]"
49 " [--square_width <square width in meter (default: 0.12)] [--help] [-h]\n"
50 << "\nExample using default camera parameters and square size:\n"
51 << " " << argv[0] << "\n"
52 << "\nExample fully tuned for a 0.1m x 0.1m square:\n"
53 << " " << argv[0] << " --intrinsic camera.xml --camera_name Camera --square_width 0.1\n"
54 << std::endl;
55 return 0;
56 }
57 }
58
60
61 // Parameters of our camera
62 vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2); // Default parameters
63 vpXmlParserCamera parser;
64 if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
65 std::cout << "Intrinsic file: " << opt_intrinsic_file << std::endl;
66 std::cout << "Camera name : " << opt_camera_name << std::endl;
67 if (parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithDistortion) == vpXmlParserCamera::SEQUENCE_OK) {
68 std::cout << "Succeed to read camera parameters from xml file" << std::endl;
69 } else {
70 std::cout << "Unable to read camera parameters from xml file" << std::endl;
71 }
72 }
73
75#if defined(VISP_HAVE_V4L2)
77 std::ostringstream device;
78 device << "/dev/video" << opt_device;
79 std::cout << "Use Video 4 Linux grabber on device " << device.str() << std::endl;
80 g.setDevice(device.str());
81 g.setScale(1);
82 g.open(I);
83#elif defined(VISP_HAVE_DC1394)
84 (void)opt_device; // To avoid non used warning
85 std::cout << "Use DC1394 grabber" << std::endl;
87 g.open(I);
88#elif defined(VISP_HAVE_CMU1394)
89 (void)opt_device; // To avoid non used warning
90 std::cout << "Use CMU1394 grabber" << std::endl;
92 g.open(I);
93#elif defined(VISP_HAVE_FLYCAPTURE)
94 (void)opt_device; // To avoid non used warning
95 std::cout << "Use FlyCapture grabber" << std::endl;
97 g.open(I);
98#elif defined(VISP_HAVE_REALSENSE2)
99 (void)opt_device; // To avoid non used warning
100 std::cout << "Use Realsense 2 grabber" << std::endl;
101 vpRealSense2 g;
102 rs2::config config;
103 config.disable_stream(RS2_STREAM_DEPTH);
104 config.disable_stream(RS2_STREAM_INFRARED);
105 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
106 g.open(config);
107 g.acquire(I);
108
109 std::cout << "Read camera parameters from Realsense device" << std::endl;
111#elif defined(VISP_HAVE_OPENCV)
112 std::cout << "Use OpenCV grabber on device " << opt_device << std::endl;
113 cv::VideoCapture g(opt_device); // Open the default camera
114 if (!g.isOpened()) { // Check if we succeeded
115 std::cout << "Failed to open the camera" << std::endl;
116 return -1;
117 }
118 cv::Mat frame;
119 g >> frame; // get a new frame from camera
120 vpImageConvert::convert(frame, I);
121#endif
123
124 std::cout << "Square width : " << opt_square_width << std::endl;
125 std::cout << cam << std::endl;
126
127 // The pose container
129
130 std::vector<vpDot2> dot(4);
131 std::vector<vpPoint> point; // 3D coordinates of the points
132 std::vector<vpImagePoint> ip; // 2D coordinates of the points in pixels
133 double L = opt_square_width / 2.;
134 point.push_back(vpPoint(-L, -L, 0));
135 point.push_back(vpPoint( L, -L, 0));
136 point.push_back(vpPoint( L, L, 0));
137 point.push_back(vpPoint(-L, L, 0));
138
139#if defined(VISP_HAVE_X11)
140 vpDisplayX d(I);
141#elif defined(VISP_HAVE_GDI)
142 vpDisplayGDI d(I);
143#elif defined(VISP_HAVE_OPENCV)
144 vpDisplayOpenCV d(I);
145#endif
146
147 bool quit = false;
148 bool apply_cv = false; // apply computer vision
149 bool init_cv = true; // initialize tracking and pose computation
150
151 while (! quit) {
152 double t_begin = vpTime::measureTimeMs();
153 // Image Acquisition
154#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_FLYCAPTURE) || defined(VISP_HAVE_REALSENSE2)
155 g.acquire(I);
156#elif defined(VISP_HAVE_OPENCV)
157 g >> frame;
158 vpImageConvert::convert(frame, I);
159#endif
161 if (apply_cv) {
162 try {
163 ip = track(I, dot, init_cv);
164 computePose(point, ip, cam, init_cv, cMo);
165 vpDisplay::displayFrame(I, cMo, cam, opt_square_width, vpColor::none, 3);
166 if (init_cv)
167 init_cv = false; // turn off the computer vision initialisation specific stuff
168
169 { // Display estimated pose in [m] and [deg]
170 vpPoseVector pose(cMo);
171 std::stringstream ss;
172 ss << "Translation: " << std::setprecision(5) << pose[0] << " " << pose[1] << " " << pose[2] << " [m]";
173 vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
174 ss.str(""); // erase ss
175 ss << "Rotation tu: " << std::setprecision(4) << vpMath::deg(pose[3]) << " " << vpMath::deg(pose[4]) << " " << vpMath::deg(pose[5]) << " [deg]";
176 vpDisplay::displayText(I, 80, 20, ss.str(), vpColor::red);
177 }
178 }
179 catch(...) {
180 std::cout << "Computer vision failure." << std::endl;
181 apply_cv = false;
182 init_cv = true;
183 }
184 }
185 vpDisplay::displayText(I, 20, 20, "Right click: quit", vpColor::red);
186 if (apply_cv) {
187 vpDisplay::displayText(I, 40, 20, "Computer vision in progress...", vpColor::red);
188 } else {
189 vpDisplay::displayText(I, 40, 20, "Left click : start", vpColor::red);
190 }
192 if (vpDisplay::getClick(I, button, false)) {
193 if (button == vpMouseButton::button3) {
194 quit = true;
195 }
196 else if (button == vpMouseButton::button1) {
197 apply_cv = true;
198 }
199 }
200 {
201 std::stringstream ss;
202 ss << "Time: " << vpTime::measureTimeMs() - t_begin << " ms";
203 vpDisplay::displayText(I, 20, I.getWidth()-100, ss.str(), vpColor::red);
204 }
206 }
207 } catch (const vpException &e) {
208 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
209 }
210#elif (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
211 (void) argc;
212 (void) argv;
213 std::cout << "Install a 3rd party dedicated to frame grabbing (dc1394, cmu1394, v4l2, OpenCV, FlyCapture, Realsense2), configure and build ViSP again to use this example" << std::endl;
214#else
215 (void) argc;
216 (void) argv;
217 std::cout << "Install a 3rd party dedicated to image display (X11, GDI, OpenCV), configure and build ViSP again to use this example" << std::endl;
218#endif
219}
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.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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:135
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(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))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:90
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:246
unsigned int getHeight() const
Definition: vpImage.h:188
static double deg(double rad)
Definition: vpMath.h:103
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
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)
VISP_EXPORT double measureTimeMs()