Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoPioneerPoint2DDepthWithoutVpServo.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35#include <iostream>
36
37#include <visp3/blob/vpDot2.h>
38#include <visp3/core/vpCameraParameters.h>
39#include <visp3/core/vpConfig.h>
40#include <visp3/core/vpHomogeneousMatrix.h>
41#include <visp3/core/vpImage.h>
42#include <visp3/core/vpImageConvert.h>
43#include <visp3/core/vpVelocityTwistMatrix.h>
44#include <visp3/gui/vpDisplayGDI.h>
45#include <visp3/gui/vpDisplayX.h>
46#include <visp3/robot/vpRobotPioneer.h> // Include first to avoid build issues with Status, None, isfinite
47#include <visp3/sensor/vp1394CMUGrabber.h>
48#include <visp3/sensor/vp1394TwoGrabber.h>
49#include <visp3/sensor/vpV4l2Grabber.h>
50#include <visp3/visual_features/vpFeatureBuilder.h>
51#include <visp3/visual_features/vpFeatureDepth.h>
52#include <visp3/visual_features/vpFeaturePoint.h>
53
54#if defined(HAVE_OPENCV_VIDEOIO)
55#include <opencv2/videoio.hpp>
56#endif
57
58#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || defined(HAVE_OPENCV_VIDEOIO)
59#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
60#if defined(VISP_HAVE_PIONEER)
61#define TEST_COULD_BE_ACHIEVED
62#endif
63#endif
64#endif
65
66#undef VISP_HAVE_OPENCV // To use a firewire camera
67#undef VISP_HAVE_V4L2 // To use a firewire camera
68
91#ifdef TEST_COULD_BE_ACHIEVED
92int main(int argc, char **argv)
93{
94 try {
95 vpImage<unsigned char> I; // Create a gray level image container
96 double depth = 1.;
97 double lambda = 0.6;
98 double coef = 1. / 6.77; // Scale parameter used to estimate the depth Z
99 // of the blob from its surface
100
101 vpRobotPioneer robot;
102 ArArgumentParser parser(&argc, argv);
103 parser.loadDefaultArguments();
104
105 // ArRobotConnector connects to the robot, get some initial data from it
106 // such as type and name, and then loads parameter files for this robot.
107 ArRobotConnector robotConnector(&parser, &robot);
108 if (!robotConnector.connectRobot()) {
109 ArLog::log(ArLog::Terse, "Could not connect to the robot.");
110 if (parser.checkHelpAndWarnUnparsed()) {
111 Aria::logOptions();
112 Aria::exit(1);
113 }
114 }
115 if (!Aria::parseArgs()) {
116 Aria::logOptions();
117 Aria::shutdown();
118 return false;
119 }
120
121 // Wait 3 sec to be sure that the low level Aria thread used to control
122 // the robot is started. Without this delay we experienced a delay
123 // (around 2.2 sec) between the velocity send to the robot and the
124 // velocity that is really applied to the wheels.
125 vpTime::sleepMs(3000);
126
127 std::cout << "Robot connected" << std::endl;
128
129 // Camera parameters. In this experiment we don't need a precise
130 // calibration of the camera
132
133 // Create the camera framegrabber
134#if defined(HAVE_OPENCV_VIDEOIO)
135 int device = 1;
136 std::cout << "Use device: " << device << std::endl;
137 cv::VideoCapture g(device); // open the default camera
138 g.set(CV_CAP_PROP_FRAME_WIDTH, 640);
139 g.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
140 if (!g.isOpened()) // check if we succeeded
141 return EXIT_FAILURE;
142 cv::Mat frame;
143 g >> frame; // get a new frame from camera
144 vpImageConvert::convert(frame, I);
145
146 // Logitec sphere parameters
147 cam.initPersProjWithoutDistortion(558, 555, 312, 210);
148#elif defined(VISP_HAVE_V4L2)
149 // Create a grabber based on v4l2 third party lib (for usb cameras under
150 // Linux)
152 g.setScale(1);
153 g.setInput(0);
154 g.setDevice("/dev/video1");
155 g.open(I);
156 // Logitec sphere parameters
157 cam.initPersProjWithoutDistortion(558, 555, 312, 210);
158#elif defined(VISP_HAVE_DC1394)
159 // Create a grabber based on libdc1394-2.x third party lib (for firewire
160 // cameras under Linux)
161 vp1394TwoGrabber g(false);
164 // AVT Pike 032C parameters
165 cam.initPersProjWithoutDistortion(800, 795, 320, 216);
166#elif defined(VISP_HAVE_CMU1394)
167 // Create a grabber based on CMU 1394 third party lib (for firewire
168 // cameras under windows)
170 g.setVideoMode(0, 5); // 640x480 MONO8
171 g.setFramerate(4); // 30 Hz
172 g.open(I);
173 // AVT Pike 032C parameters
174 cam.initPersProjWithoutDistortion(800, 795, 320, 216);
175#endif
176
177 // Acquire an image from the grabber
178#if defined(HAVE_OPENCV_VIDEOIO)
179 g >> frame; // get a new frame from camera
180 vpImageConvert::convert(frame, I);
181#else
182 g.acquire(I);
183#endif
184
185 // Create an image viewer
186#if defined(VISP_HAVE_X11)
187 vpDisplayX d(I, 10, 10, "Current frame");
188#elif defined(VISP_HAVE_GDI)
189 vpDisplayGDI d(I, 10, 10, "Current frame");
190#endif
193
194 // Create a blob tracker
195 vpDot2 dot;
196 dot.setGraphics(true);
197 dot.setComputeMoments(true);
198 dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
199 dot.setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation
200 dot.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner
201 // and outside points with bad
202 // gray level
203 dot.initTracking(I);
205
206 // Current and desired visual feature associated to the x coordinate of
207 // the point
208 vpFeaturePoint s_x, s_xd;
209
210 // Create the current x visual feature
211 vpFeatureBuilder::create(s_x, cam, dot);
212
213 // Create the desired x* visual feature
214 s_xd.buildFrom(0, 0, depth);
216
217 // Create the current log(Z/Z*) visual feature
218 vpFeatureDepth s_Z;
219 // Surface of the blob estimated from the image moment m00 and converted
220 // in meters
221 double surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
222 double Z, Zd;
223 // Initial depth of the blob in from of the camera
224 Z = coef * surface;
225 // Desired depth Z* of the blob. This depth is learned and equal to the
226 // initial depth
227 Zd = Z;
228 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z,
229 0); // log(Z/Z*) = 0 that's why the last parameter is 0
230 vpMatrix L_Z = s_Z.interaction();
231
232 vpVelocityTwistMatrix cVe = robot.get_cVe();
233 vpMatrix eJe; // pioneer jacobian
234 robot.get_eJe(eJe);
235
236 vpMatrix L; // Interaction matrix
237 L.stack(L_x); // constant since build with the desired feature
238 L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that
239 // evolves at each iteration
240
241 vpColVector v; // vz, wx
242
243 vpFeatureDepth s_Zd;
244 s_Zd.buildFrom(0, 0, 1, 0); // The value of s* is 0 with Z=1 meter.
245
246 while (1) {
247 // Acquire a new image
248#if defined(HAVE_OPENCV_VIDEOIO)
249 g >> frame; // get a new frame from camera
250 vpImageConvert::convert(frame, I);
251#else
252 g.acquire(I);
253#endif
254 // Set the image as background of the viewer
256
257 // Does the blob tracking
258 dot.track(I);
259 // Update the current x feature
260 vpFeatureBuilder::create(s_x, cam, dot);
261
262 // Update log(Z/Z*) feature. Since the depth Z change, we need to update
263 // the intection matrix
264 surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
265 Z = coef * surface;
266 s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
267 L_Z = s_Z.interaction();
268
269 // Update the global interaction matrix
270 vpMatrix L;
271 L.stack(L_x); // constant since build with the desired feature
272 L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that
273 // evolves at each iteration
274
275// Update the global error s-s*
276 vpColVector error;
277 error.stack(s_x.error(s_xd, vpFeaturePoint::selectX()));
278 error.stack(s_Z.error(s_Zd));
279
280 // Compute the control law. Velocities are computed in the mobile robot
281 // reference frame
282 v = -lambda * (L * cVe * eJe).pseudoInverse() * error;
283
284 std::cout << "Send velocity to the pionner: " << v[0] << " m/s " << vpMath::deg(v[1]) << " deg/s" << std::endl;
285
286 // Send the velocity to the robot
288
289 // Draw a vertical line which corresponds to the desired x coordinate of
290 // the dot cog
291 vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
293
294 // A click in the viewer to exit
295 if (vpDisplay::getClick(I, false))
296 break;
297 }
298
299 std::cout << "Ending robot thread..." << std::endl;
300 robot.stopRunning();
301
302 // wait for the thread to stop
303 robot.waitForRunExit();
304 return EXIT_SUCCESS;
305 }
306 catch (const vpException &e) {
307 std::cout << "Catch an exception: " << e << std::endl;
308 return EXIT_FAILURE;
309 }
310}
311#else
312int main()
313{
314 std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
315 return EXIT_SUCCESS;
316}
317#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.
void stack(double d)
static const vpColor red
Definition vpColor.h:211
Display for windows using GDI (available on any windows 32 platform).
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 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:124
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition vpDot2.cpp:441
void setGraphics(bool activate)
Definition vpDot2.h:311
double m00
Definition vpDot2.h:372
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition vpDot2.cpp:717
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Definition vpDot2.h:287
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Definition vpDot2.cpp:788
void setComputeMoments(bool activate)
Definition vpDot2.h:273
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition vpDot2.cpp:252
error that can be emitted by ViSP classes.
Definition vpException.h:59
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)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
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)
static unsigned int selectX()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
double get_y() const
double get_x() const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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:74
vpVelocityTwistMatrix get_cVe() const
Definition vpUnicycle.h:79
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)