Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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  } catch (const vpException &e) {
180  std::cout << "Catch an exception: " << e << std::endl;
181  }
182 #endif
183 }
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void init(const vpImage< unsigned char > &I, vpColVector *X)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:134
void acquire(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam)
vpHomogeneousMatrix getPosition() const
static const vpColor green
Definition: vpColor.h:220
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
virtual void setSamplingTime(const double &delta_t)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
void setInterpolationType(const vpInterpolationType interplt)
void set_Z(double Z)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:404
Class which enables to project an image in the 3D space and get the view of a virtual camera...
double getSamplingTime() const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
static double rad(double deg)
Definition: vpMath.h:110
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setCleanPreviousImage(const bool &clean, const vpColor &color=vpColor::white)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
unsigned int getWidth() const
Definition: vpImage.h:246
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
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 display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)