Visual Servoing Platform  version 3.2.0
tutorial-ibvs-4pts-image-tracking.cpp
1 
2 #include <visp3/gui/vpDisplayGDI.h>
3 #include <visp3/gui/vpDisplayOpenCV.h>
4 #include <visp3/gui/vpDisplayX.h>
5 #include <visp3/io/vpImageIo.h>
6 #include <visp3/robot/vpImageSimulator.h>
7 #include <visp3/robot/vpSimulatorCamera.h>
8 #include <visp3/visual_features/vpFeatureBuilder.h>
9 #include <visp3/vs/vpServo.h>
10 #include <visp3/vs/vpServoDisplay.h>
11 
12 void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot);
13 
20 {
21 public:
27  vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam) : sim_(), target_(), cam_()
28  {
29  // The target is a square 20cm by 2cm square
30  // Initialise the 3D coordinates of the target corners
31  for (int i = 0; i < 4; i++)
32  X_[i].resize(3);
33  // Top left Top right Bottom right Bottom left
34  X_[0][0] = -0.1;
35  X_[1][0] = 0.1;
36  X_[2][0] = 0.1;
37  X_[3][0] = -0.1;
38  X_[0][1] = -0.1;
39  X_[1][1] = -0.1;
40  X_[2][1] = 0.1;
41  X_[3][1] = 0.1;
42  X_[0][2] = 0;
43  X_[1][2] = 0;
44  X_[2][2] = 0;
45  X_[3][2] = 0;
46 
47  vpImageIo::read(target_, filename);
48 
49  // Initialize the image simulator
50  cam_ = cam;
52  sim_.init(target_, X_);
53  }
54 
64  {
65  sim_.setCleanPreviousImage(true);
66  sim_.setCameraPosition(cMo);
67  sim_.getImage(I, cam_);
68  }
69 
70 private:
71  vpColVector X_[4]; // 3D coordinates of the target corners
72  vpImageSimulator sim_;
73  vpImage<unsigned char> target_; // image of the target
74  vpCameraParameters cam_;
75 };
76 
77 void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot)
78 {
79  static std::vector<vpImagePoint> traj[4];
80  for (unsigned int i = 0; i < 4; i++) {
81  traj[i].push_back(dot[i].getCog());
82  }
83  for (unsigned int i = 0; i < 4; i++) {
84  for (unsigned int j = 1; j < traj[i].size(); j++) {
85  vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green);
86  }
87  }
88 }
89 
90 int main()
91 {
92 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
93  try {
94  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
95  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
96 
97  vpImage<unsigned char> I(480, 640, 255);
98  vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
99 
100  std::vector<vpPoint> point;
101  point.push_back(vpPoint(-0.1, -0.1, 0));
102  point.push_back(vpPoint(0.1, -0.1, 0));
103  point.push_back(vpPoint(0.1, 0.1, 0));
104  point.push_back(vpPoint(-0.1, 0.1, 0));
105 
106  vpServo task;
109  task.setLambda(0.5);
110 
111  vpVirtualGrabber g("./target_square.pgm", cam);
112  g.acquire(I, cMo);
113 
114 #if defined(VISP_HAVE_X11)
115  vpDisplayX d(I, 0, 0, "Current camera view");
116 #elif defined(VISP_HAVE_GDI)
117  vpDisplayGDI d(I, 0, 0, "Current camera view");
118 #elif defined(VISP_HAVE_OPENCV)
119  vpDisplayOpenCV d(I, 0, 0, "Current camera view");
120 #else
121  std::cout << "No image viewer is available..." << std::endl;
122 #endif
123 
125  vpDisplay::displayText(I, 10, 10, "Click in the 4 dots to initialise the tracking and start the servo",
126  vpColor::red);
127  vpDisplay::flush(I);
128 
129  vpFeaturePoint p[4], pd[4];
130  std::vector<vpDot2> dot(4);
131 
132  for (unsigned int i = 0; i < 4; i++) {
133  point[i].track(cdMo);
134  vpFeatureBuilder::create(pd[i], point[i]);
135 
136  dot[i].setGraphics(true);
137  dot[i].initTracking(I);
138  vpDisplay::flush(I);
139  vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
140 
141  task.addFeature(p[i], pd[i]);
142  }
143 
144  vpHomogeneousMatrix wMc, wMo;
145  vpSimulatorCamera robot;
146  robot.setSamplingTime(0.040);
147  robot.getPosition(wMc);
148  wMo = wMc * cMo;
149 
150  for (;;) {
151  robot.getPosition(wMc);
152  cMo = wMc.inverse() * wMo;
153 
154  g.acquire(I, cMo);
155 
157 
158  for (unsigned int i = 0; i < 4; i++) {
159  dot[i].track(I);
160  vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
161 
162  vpColVector cP;
163  point[i].changeFrame(cMo, cP);
164  p[i].set_Z(cP[2]);
165  }
166 
167  vpColVector v = task.computeControlLaw();
168 
169  display_trajectory(I, dot);
172 
173  vpDisplay::flush(I);
174  if (vpDisplay::getClick(I, false))
175  break;
176 
177  vpTime::wait(robot.getSamplingTime() * 1000);
178  }
179  task.kill();
180  } catch (const vpException &e) {
181  std::cout << "Catch an exception: " << e << std::endl;
182  }
183 #endif
184 }
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
vpVirtualGrabber::acquire
void acquire(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: tutorial-ibvs-4pts-image-tracking.cpp:63
vpServo::kill
void kill()
Definition: vpServo.cpp:191
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:149
vpServo::CURRENT
Definition: vpServo.h:185
vpImageSimulator::setInterpolationType
void setInterpolationType(const vpInterpolationType interplt)
Definition: vpImageSimulator.h:230
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:101
vpVirtualGrabber
Definition: tutorial-ibvs-4pts-image-tracking.cpp:19
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpImageSimulator::setCameraPosition
void setCameraPosition(const vpHomogeneousMatrix &cMt)
Definition: vpImageSimulator.cpp:967
vpImageIo::read
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:206
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:405
vpRobotSimulator::getSamplingTime
double getSamplingTime() const
Definition: vpRobotSimulator.h:81
vpServo::EYEINHAND_CAMERA
Definition: vpServo.h:158
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:92
vpImageSimulator::init
void init(const vpImage< unsigned char > &I, vpColVector *X)
Definition: vpImageSimulator.cpp:1080
vpImageSimulator
Class which enables to project an image in the 3D space and get the view of a virtual camera.
Definition: vpImageSimulator.h:141
vpFeaturePoint::set_Z
void set_Z(const double Z)
Definition: vpFeaturePoint.cpp:96
vpImageSimulator::setCleanPreviousImage
void setCleanPreviousImage(const bool &clean, const vpColor &color=vpColor::white)
Definition: vpImageSimulator.h:259
vpSimulatorCamera
Class that defines the simplest robot: a free flying camera.
Definition: vpSimulatorCamera.h:106
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:71
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:141
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpSimulatorCamera::getPosition
vpHomogeneousMatrix getPosition() const
Definition: vpSimulatorCamera.cpp:118
vpColor::green
static const vpColor green
Definition: vpColor.h:182
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:676
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay_uchar.cpp:600
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:177
vpVirtualGrabber::vpVirtualGrabber
vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam)
Definition: tutorial-ibvs-4pts-image-tracking.cpp:27
vpImageSimulator::BILINEAR_INTERPOLATION
Definition: vpImageSimulator.h:146
vpRobot::CAMERA_FRAME
Definition: vpRobot.h:81
vpFeaturePoint
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Definition: vpFeaturePoint.h:180
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:573
vpImageSimulator::getImage
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
Definition: vpImageSimulator.cpp:188
vpServo
Definition: vpServo.h:149
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:934
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:652
vpRobotSimulator::setSamplingTime
virtual void setSamplingTime(const double &delta_t)
Definition: vpRobotSimulator.h:90
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:642
vpImage< unsigned char >
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:701
vpDisplay::displayLine
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:391
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:197
vpServoDisplay::display
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
Definition: vpServoDisplay.cpp:79
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpColor::red
static const vpColor red
Definition: vpColor.h:179
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:238