Visual Servoing Platform version 3.5.0
servoPioneerPoint2DDepth.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * IBVS on Pioneer P3DX mobile platform
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38#include <iostream>
39
40#include <visp3/core/vpConfig.h>
41
42#include <visp3/robot/vpRobotPioneer.h> // Include first to avoid build issues with Status, None, isfinite
43#include <visp3/blob/vpDot2.h>
44#include <visp3/core/vpCameraParameters.h>
45#include <visp3/core/vpHomogeneousMatrix.h>
46#include <visp3/core/vpImage.h>
47#include <visp3/core/vpImageConvert.h>
48#include <visp3/core/vpVelocityTwistMatrix.h>
49#include <visp3/gui/vpDisplayGDI.h>
50#include <visp3/gui/vpDisplayX.h>
51#include <visp3/sensor/vp1394CMUGrabber.h>
52#include <visp3/sensor/vp1394TwoGrabber.h>
53#include <visp3/sensor/vpOpenCVGrabber.h>
54#include <visp3/sensor/vpV4l2Grabber.h>
55#include <visp3/visual_features/vpFeatureBuilder.h>
56#include <visp3/visual_features/vpFeatureDepth.h>
57#include <visp3/visual_features/vpFeaturePoint.h>
58#include <visp3/vs/vpServo.h>
59
60#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || \
61 (VISP_HAVE_OPENCV_VERSION >= 0x020100)
62#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
63#if defined(VISP_HAVE_PIONEER)
64#define TEST_COULD_BE_ACHIEVED
65#endif
66#endif
67#endif
68
69#undef VISP_HAVE_OPENCV // To use a firewire camera
70#undef VISP_HAVE_V4L2 // To use a firewire camera
71
94#ifdef TEST_COULD_BE_ACHIEVED
95int main(int argc, char **argv)
96{
97 try {
98 vpImage<unsigned char> I; // Create a gray level image container
99 double depth = 1.;
100 double lambda = 0.6;
101 double coef = 1. / 6.77; // Scale parameter used to estimate the depth Z
102 // of the blob from its surface
103
104 vpRobotPioneer robot;
105 ArArgumentParser parser(&argc, argv);
106 parser.loadDefaultArguments();
107
108 // ArRobotConnector connects to the robot, get some initial data from it
109 // such as type and name, and then loads parameter files for this robot.
110 ArRobotConnector robotConnector(&parser, &robot);
111 if (!robotConnector.connectRobot()) {
112 ArLog::log(ArLog::Terse, "Could not connect to the robot.");
113 if (parser.checkHelpAndWarnUnparsed()) {
114 Aria::logOptions();
115 Aria::exit(1);
116 }
117 }
118 if (!Aria::parseArgs()) {
119 Aria::logOptions();
120 Aria::shutdown();
121 return false;
122 }
123
124 // Wait 3 sec to be sure that the low level Aria thread used to control
125 // the robot is started. Without this delay we experienced a delay
126 // (arround 2.2 sec) between the velocity send to the robot and the
127 // velocity that is really applied to the wheels.
128 vpTime::sleepMs(3000);
129
130 std::cout << "Robot connected" << std::endl;
131
132 // Camera parameters. In this experiment we don't need a precise
133 // calibration of the camera
135
136// Create the camera framegrabber
137#if defined(VISP_HAVE_OPENCV)
138 int device = 1;
139 std::cout << "Use device: " << device << std::endl;
140 cv::VideoCapture g(device); // open the default camera
141 g.set(CV_CAP_PROP_FRAME_WIDTH, 640);
142 g.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
143 if (!g.isOpened()) // check if we succeeded
144 return -1;
145 cv::Mat frame;
146 g >> frame; // get a new frame from camera
147 vpImageConvert::convert(frame, I);
148
149 // Logitec sphere parameters
150 cam.initPersProjWithoutDistortion(558, 555, 312, 210);
151#elif defined(VISP_HAVE_V4L2)
152 // Create a grabber based on v4l2 third party lib (for usb cameras under
153 // Linux)
155 g.setScale(1);
156 g.setInput(0);
157 g.setDevice("/dev/video1");
158 g.open(I);
159 // Logitec sphere parameters
160 cam.initPersProjWithoutDistortion(558, 555, 312, 210);
161#elif defined(VISP_HAVE_DC1394)
162 // Create a grabber based on libdc1394-2.x third party lib (for firewire
163 // cameras under Linux)
164 vp1394TwoGrabber g(false);
167 // AVT Pike 032C parameters
168 cam.initPersProjWithoutDistortion(800, 795, 320, 216);
169#elif defined(VISP_HAVE_CMU1394)
170 // Create a grabber based on CMU 1394 third party lib (for firewire
171 // cameras under windows)
173 g.setVideoMode(0, 5); // 640x480 MONO8
174 g.setFramerate(4); // 30 Hz
175 g.open(I);
176 // AVT Pike 032C parameters
177 cam.initPersProjWithoutDistortion(800, 795, 320, 216);
178#endif
179
180// Acquire an image from the grabber
181#if defined(VISP_HAVE_OPENCV)
182 g >> frame; // get a new frame from camera
183 vpImageConvert::convert(frame, I);
184#else
185 g.acquire(I);
186#endif
187
188// Create an image viewer
189#if defined(VISP_HAVE_X11)
190 vpDisplayX d(I, 10, 10, "Current frame");
191#elif defined(VISP_HAVE_GDI)
192 vpDisplayGDI d(I, 10, 10, "Current frame");
193#endif
196
197 // Create a blob tracker
198 vpDot2 dot;
199 dot.setGraphics(true);
200 dot.setComputeMoments(true);
201 dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
202 dot.setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation
203 dot.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner
204 // and outside points with bad
205 // gray level
206 dot.initTracking(I);
208
209 vpServo task;
212 task.setLambda(lambda);
214 cVe = robot.get_cVe();
215 task.set_cVe(cVe);
216
217 std::cout << "cVe: \n" << cVe << std::endl;
218
219 vpMatrix eJe;
220 robot.get_eJe(eJe);
221 task.set_eJe(eJe);
222 std::cout << "eJe: \n" << eJe << std::endl;
223
224 // Current and desired visual feature associated to the x coordinate of
225 // the point
226 vpFeaturePoint s_x, s_xd;
227
228 // Create the current x visual feature
229 vpFeatureBuilder::create(s_x, cam, dot);
230
231 // Create the desired x* visual feature
232 s_xd.buildFrom(0, 0, depth);
233
234 // Add the feature
235 task.addFeature(s_x, s_xd);
236
237 // Create the current log(Z/Z*) visual feature
238 vpFeatureDepth s_Z, s_Zd;
239 // Surface of the blob estimated from the image moment m00 and converted
240 // in meters
241 double surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
242 double Z, Zd;
243 // Initial depth of the blob in from of the camera
244 Z = coef * surface;
245 // Desired depth Z* of the blob. This depth is learned and equal to the
246 // initial depth
247 Zd = Z;
248
249 std::cout << "Z " << Z << std::endl;
250 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z,
251 0); // log(Z/Z*) = 0 that's why the last parameter is 0
252 s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd,
253 0); // log(Z/Z*) = 0 that's why the last parameter is 0
254
255 // Add the feature
256 task.addFeature(s_Z, s_Zd);
257
258 vpColVector v; // vz, wx
259
260 while (1) {
261// Acquire a new image
262#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
263 g >> frame; // get a new frame from camera
264 vpImageConvert::convert(frame, I);
265#else
266 g.acquire(I);
267#endif
268 // Set the image as background of the viewer
270
271 // Does the blob tracking
272 dot.track(I);
273 // Update the current x feature
274 vpFeatureBuilder::create(s_x, cam, dot);
275
276 // Update log(Z/Z*) feature. Since the depth Z change, we need to update
277 // the intection matrix
278 surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
279 Z = coef * surface;
280 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
281
282 robot.get_cVe(cVe);
283 task.set_cVe(cVe);
284
285 robot.get_eJe(eJe);
286 task.set_eJe(eJe);
287
288 // Compute the control law. Velocities are computed in the mobile robot
289 // reference frame
290 v = task.computeControlLaw();
291
292 std::cout << "Send velocity to the pionner: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
293
294 // Send the velocity to the robot
296
297 // Draw a vertical line which corresponds to the desired x coordinate of
298 // the dot cog
299 vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
301
302 // A click in the viewer to exit
303 if (vpDisplay::getClick(I, false))
304 break;
305 }
306
307 std::cout << "Ending robot thread..." << std::endl;
308 robot.stopRunning();
309
310 // wait for the thread to stop
311 robot.waitForRunExit();
312
313 // Kill the servo task
314 task.print();
315 return EXIT_SUCCESS;
316 } catch (const vpException &e) {
317 std::cout << "Catch an exception: " << e << std::endl;
318 return EXIT_FAILURE;
319 }
320}
321#else
322int main()
323{
324 std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
325 return EXIT_SUCCESS;
326}
327#endif
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setVideoMode(unsigned long format, unsigned long mode)
void acquire(vpImage< unsigned char > &I)
void setFramerate(unsigned long fps)
void open(vpImage< unsigned char > &I)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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 displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:127
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition: vpDot2.cpp:441
void setGraphics(bool activate)
Definition: vpDot2.h:314
double m00
Definition: vpDot2.h:375
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition: vpDot2.cpp:735
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Definition: vpDot2.h:290
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Definition: vpDot2.cpp:806
void setComputeMoments(bool activate)
Definition: vpDot2.h:276
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:253
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void buildFrom(double x, double y, double Z, double LogZoverZstar)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
double get_y() const
double get_x() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Interface for Pioneer mobile robots based on Aria 3rd party library.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ REFERENCE_FRAME
Definition: vpRobot.h:76
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:306
void setLambda(double c)
Definition: vpServo.h:404
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
@ PSEUDO_INVERSE
Definition: vpServo.h:202
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ DESIRED
Definition: vpServo.h:186
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:82
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setFramerate(vpV4l2FramerateType framerate)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
VISP_EXPORT void sleepMs(double t)