ViSP  2.6.2
servoPioneerPoint2DDepth.cpp
1 /****************************************************************************
2  *
3  * $Id: servoPioneerPoint2DDepth.cpp 3820 2012-06-27 13:13:29Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * IBVS on Pioneer P3DX mobile platform
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 #include <iostream>
42 
43 #include <visp/vpConfig.h>
44 
45 #include <visp/vpRobotPioneer.h>
46 #include <visp/vpCameraParameters.h>
47 #include <visp/vpDisplayGDI.h>
48 #include <visp/vpDisplayX.h>
49 #include <visp/vpDot2.h>
50 #include <visp/vpFeatureBuilder.h>
51 #include <visp/vpFeatureDepth.h>
52 #include <visp/vpFeaturePoint.h>
53 #include <visp/vpHomogeneousMatrix.h>
54 #include <visp/vpImage.h>
55 #include <visp/vpImageConvert.h>
56 #include <visp/vp1394TwoGrabber.h>
57 #include <visp/vp1394CMUGrabber.h>
58 #include <visp/vpV4l2Grabber.h>
59 #include <visp/vpOpenCVGrabber.h>
60 #include <visp/vpServo.h>
61 #include <visp/vpVelocityTwistMatrix.h>
62 
63 #if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_OPENCV)
64 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
65 #if defined(VISP_HAVE_PIONEER)
66 # define TEST_COULD_BE_ACHIEVED
67 #endif
68 #endif
69 #endif
70 
90 #ifdef TEST_COULD_BE_ACHIEVED
91 int main(int argc, char **argv)
92 {
93  vpImage<unsigned char> I; // Create a gray level image container
94  double depth = 1.;
95  double lambda = 0.6;
96  double coef = 1./6.77; // Scale parameter used to estimate the depth Z of the blob from its surface
97 
98  vpRobotPioneer robot;
99  ArArgumentParser parser(&argc, argv);
100  parser.loadDefaultArguments();
101 
102  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
103  // and then loads parameter files for this robot.
104  ArRobotConnector robotConnector(&parser, &robot);
105  if(!robotConnector.connectRobot())
106  {
107  ArLog::log(ArLog::Terse, "Could not connect to the robot.");
108  if(parser.checkHelpAndWarnUnparsed())
109  {
110  Aria::logOptions();
111  Aria::exit(1);
112  }
113  }
114  if (!Aria::parseArgs())
115  {
116  Aria::logOptions();
117  Aria::shutdown();
118  return false;
119  }
120 
121  std::cout << "Robot connected" << std::endl;
122 
123  // Create the camera framegrabber
124 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
125  int device = 1;
126  std::cout << "Use device: " << device << std::endl;
127  cv::VideoCapture g(device); // open the default camera
128  g.set(CV_CAP_PROP_FRAME_WIDTH, 640);
129  g.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
130  if(!g.isOpened()) // check if we succeeded
131  return -1;
132  cv::Mat frame;
133  g >> frame; // get a new frame from camera
134  vpImageConvert::convert(frame, I);
135 #elif defined(VISP_HAVE_V4L2)
136  // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
137  vpV4l2Grabber g;
138  g.setScale(1);
139  g.setInput(0);
140  g.setDevice("/dev/video1");
141  g.open(I);
142 #elif defined(VISP_HAVE_DC1394_2)
143  // Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
144  vp1394TwoGrabber g(false);
147 #elif defined(VISP_HAVE_CMU1394)
148  // Create a grabber based on CMU 1394 third party lib (for firewire cameras under windows)
150  g.setVideoMode(0, 5); // 640x480 MONO8
151  g.setFramerate(4); // 30 Hz
152  g.open(I);
153 #endif
154 
155  // Acquire an image from the grabber
156 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
157  g >> frame; // get a new frame from camera
158  vpImageConvert::convert(frame, I);
159 #else
160  g.acquire(I);
161 #endif
162 
163  // Create an image viewer
164 #if defined(VISP_HAVE_X11)
165  vpDisplayX d(I, 10, 10, "Current frame");
166 #elif defined(VISP_HAVE_GDI)
167  vpDisplayGDI d(I, 10, 10, "Current frame");
168 #endif
170  vpDisplay::flush(I);
171 
172  // Create a blob tracker
173  vpDot2 dot;
174  dot.setGraphics(true);
175  dot.setComputeMoments(true);
176  dot.setEllipsoidShapePrecision(0.); // to track a blob without any constraint on the shape
177  dot.setGrayLevelPrecision(0.9); // to set the blob gray level bounds for binarisation
178  dot.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray level
179  dot.initTracking(I);
180  vpDisplay::flush(I);
181 
182  // Camera parameters. In this experiment we don't need a precise calibration of the camera
183  vpCameraParameters cam;
184  cam.initPersProjWithoutDistortion(800, 800, I.getWidth() / 2., I.getHeight() / 2.);
185 
186  vpServo task;
189  task.setLambda(lambda) ;
191  cVe = robot.get_cVe() ;
192  task.set_cVe(cVe) ;
193 
194  std::cout << "cVe: \n" << cVe << std::endl;
195 
196  vpMatrix eJe;
197  robot.get_eJe(eJe) ;
198  task.set_eJe(eJe) ;
199  std::cout << "eJe: \n" << eJe << std::endl;
200 
201  // Current and desired visual feature associated to the x coordinate of the point
202  vpFeaturePoint s_x, s_xd;
203 
204  // Create the current x visual feature
205  vpFeatureBuilder::create(s_x, cam, dot);
206 
207  // Create the desired x* visual feature
208  s_xd.buildFrom(0, 0, depth);
209 
210  // Add the feature
211  task.addFeature(s_x, s_xd) ;
212 
213  // Create the current log(Z/Z*) visual feature
214  vpFeatureDepth s_Z, s_Zd;
215  // Surface of the blob estimated from the image moment m00 and converted in meters
216  double surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
217  double Z, Zd;
218  // Initial depth of the blob in from of the camera
219  Z = coef * surface ;
220  // Desired depth Z* of the blob. This depth is learned and equal to the initial depth
221  Zd = Z;
222 
223  std::cout << "Z " << Z << std::endl;
224  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
225  s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
226 
227  // Add the feature
228  task.addFeature(s_Z, s_Zd) ;
229 
230  vpColVector v; // vz, wx
231 
232  try
233  {
234  while(1)
235  {
236  // Acquire a new image
237 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
238  g >> frame; // get a new frame from camera
239  vpImageConvert::convert(frame, I);
240 #else
241  g.acquire(I);
242 #endif
243  // Set the image as background of the viewer
245 
246  // Does the blob tracking
247  dot.track(I);
248  // Update the current x feature
249  vpFeatureBuilder::create(s_x, cam, dot);
250 
251  // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
252  surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
253  Z = coef * surface ;
254  s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
255 
256  robot.get_cVe(cVe) ;
257  task.set_cVe(cVe) ;
258 
259  robot.get_eJe(eJe) ;
260  task.set_eJe(eJe) ;
261 
262  // Compute the control law. Velocities are computed in the mobile robot reference frame
263  v = task.computeControlLaw() ;
264 
265  std::cout << "v: " << v.t() << std::endl;
266 
267  // Send the velocity to the robot
269 
270  // Draw a vertical line which corresponds to the desired x coordinate of the dot cog
271  vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
272  vpDisplay::flush(I);
273 
274  // A click in the viewer to exit
275  if ( vpDisplay::getClick(I, false) )
276  break;
277  }
278  }
279  catch(...)
280  {
281  }
282 
283  std::cout << "Ending robot thread..." << std::endl;
284  robot.stopRunning();
285 
286  // wait for the thread to stop
287  robot.waitForRunExit();
288 
289  // Kill the servo task
290  task.print() ;
291  task.kill();
292 }
293 #else
294 int main()
295 {
296  std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
297 }
298 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void setVideoMode(unsigned long format, unsigned long mode)
void open(vpImage< unsigned char > &I)
unsigned int getWidth() const
Definition: vpImage.h:154
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void get_eJe(vpMatrix &eJe)
void open(vpImage< unsigned char > &I)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Definition: vpDot2.h:157
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:89
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
void setDevice(const char *devname)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:250
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Interface for Pioneer mobile robots based on Aria 3rd party library.
double get_py() const
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:114
void track(const vpImage< unsigned char > &I)
Definition: vpDot2.cpp:439
void set_cVe(vpVelocityTwistMatrix &_cVe)
Definition: vpServo.h:227
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
static const vpColor red
Definition: vpColor.h:165
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void setGrayLevelPrecision(const double &grayLevelPrecision)
Definition: vpDot2.cpp:763
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setFramerate(unsigned long fps)
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
void acquire(vpImage< unsigned char > &I)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
void set_eJe(vpMatrix &_eJe)
Definition: vpServo.h:235
vpRowVector t() const
transpose of Vector
Generic class defining intrinsic camera parameters.
void setComputeMoments(const bool activate)
Definition: vpDot2.h:143
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
double get_px() const
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Definition: vpDot2.cpp:838
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
void buildFrom(const double x, const double y, const double Z)
Class for the Video4Linux2 video device.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double get_y() const
double get_x() const
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:240
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:258
unsigned int getHeight() const
Definition: vpImage.h:145
Class for firewire ieee1394 video devices using libdc1394-2.x api.
virtual bool getClick(bool blocking=true)=0
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
double m00
Definition: vpDot2.h:336
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law.
Definition: vpServo.h:150
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
void setGraphics(const bool activate)
Definition: vpDot2.h:178
void setFramerate(vpV4l2FramerateType framerate)