Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
tutorial-ukf.cpp
1 
32 // ViSP includes
33 #include <visp3/core/vpConfig.h>
34 #include <visp3/core/vpCameraParameters.h>
35 #include <visp3/core/vpGaussRand.h>
36 #include <visp3/core/vpHomogeneousMatrix.h>
37 #include <visp3/core/vpMeterPixelConversion.h>
38 #include <visp3/core/vpPixelMeterConversion.h>
40 #ifdef VISP_HAVE_DISPLAY
41 #include <visp3/gui/vpPlot.h>
42 #include <visp3/gui/vpDisplayFactory.h>
43 #endif
45 #include <visp3/vision/vpPose.h>
46 
48 #include <visp3/core/vpUKSigmaDrawerMerwe.h>
49 #include <visp3/core/vpUnscentedKalman.h>
51 
52 #ifdef ENABLE_VISP_NAMESPACE
53 using namespace VISP_NAMESPACE_NAME;
54 #endif
55 
56 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
58 
65 vpColVector fx(const vpColVector &x, const double & /*dt*/)
66 {
67  vpColVector x_kPlus1(4);
68  x_kPlus1[0] = x[0] * std::cos(x[3]) - x[1] * std::sin(x[3]); // wX
69  x_kPlus1[1] = x[0] * std::sin(x[3]) + x[1] * std::cos(x[3]); // wY
70  x_kPlus1[2] = x[2]; // wZ
71  x_kPlus1[3] = x[3]; // omega * dt
72  return x_kPlus1;
73 }
75 
77 
81 {
82 public:
91  vpObjectSimulator(const double &R, const double &w, const double &phi, const double &wZ)
92  : m_R(R)
93  , m_w(w)
94  , m_phi(phi)
95  , m_wZ(wZ)
96  { }
97 
104  vpColVector move(const double &t) const
105  {
106  vpColVector wX(4, 1.);
107  wX[0] = m_R * std::cos(m_w * t + m_phi);
108  wX[1] = m_R * std::sin(m_w * t + m_phi);
109  wX[2] = m_wZ;
110  return wX;
111  }
112 
113 private:
114  double m_R; // Radius of the revolution around the world frame origin.
115  double m_w; // Pulsation of the motion.
116  double m_phi; // Phase of the motion.
117  const double m_wZ; // The z-coordinate of the object in the world frame.
118 };
120 
122 
126 {
127 public:
139  const std::vector<vpColVector> &markers, const double &noise_stdev, const long &seed)
140  : m_cam(cam)
141  , m_cMw(cMw)
142  , m_wRo(wRo)
143  , m_markers(markers)
144  , m_rng(noise_stdev, 0., seed)
145  { }
146 
148 
155  {
156  unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
157  vpColVector meas(2*nbMarkers);
159  vpTranslationVector wTo(x[0], x[1], x[2]);
160  wMo.buildFrom(wTo, m_wRo);
161  for (unsigned int i = 0; i < nbMarkers; ++i) {
162  vpColVector cX = m_cMw * wMo * m_markers[i];
163  double u = 0., v = 0.;
164  vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
165  meas[2*i] = u;
166  meas[2*i + 1] = v;
167  }
168  return meas;
169  }
171 
173 
181  {
182  unsigned int nbMarkers = static_cast<unsigned int>(m_markers.size());
183  vpColVector meas(2*nbMarkers);
185  vpTranslationVector wTo(wX[0], wX[1], wX[2]);
186  wMo.buildFrom(wTo, m_wRo);
187  for (unsigned int i = 0; i < nbMarkers; ++i) {
188  vpColVector cX = m_cMw * wMo * m_markers[i];
189  double u = 0., v = 0.;
190  vpMeterPixelConversion::convertPoint(m_cam, cX[0] / cX[2], cX[1] / cX[2], u, v);
191  meas[2*i] = u;
192  meas[2*i + 1] = v;
193  }
194  return meas;
195  }
197 
199 
207  {
208  vpColVector measurementsGT = measureGT(wX);
209  vpColVector measurementsNoisy = measurementsGT;
210  unsigned int sizeMeasurement = measurementsGT.size();
211  for (unsigned int i = 0; i < sizeMeasurement; ++i) {
212  measurementsNoisy[i] += m_rng();
213  }
214  return measurementsNoisy;
215  }
217 
218 private:
219  vpCameraParameters m_cam; // The camera parameters
220  vpHomogeneousMatrix m_cMw; // The pose of the world frame with regard to the camera frame.
221  vpRotationMatrix m_wRo; // The rotation matrix that expresses the rotation between the world frame and object frame.
222  std::vector<vpColVector> m_markers; // The position of the markers in the object frame.
223  vpGaussRand m_rng; // Noise simulator for the measurements
224 };
226 
228 
237 vpHomogeneousMatrix computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip, const vpCameraParameters &cam)
238 {
239  vpPose pose;
240  double x = 0, y = 0;
241  for (unsigned int i = 0; i < point.size(); i++) {
242  vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
243  point[i].set_x(x);
244  point[i].set_y(y);
245  pose.addPoint(point[i]);
246  }
247 
250  return cMo;
251 }
253 
254 int main(/*const int argc, const char *argv[]*/)
255 {
257  const double dt = 0.001; // Period of 0.1s
258  const double sigmaMeasurements = 2.; // Standard deviation of the measurements: 2 pixels
259  const double radius = 0.25; // Radius of revolution of 0.25m
260  const double w = 2 * M_PI * 10; // Pulsation of the motion of revolution
261  const double phi = 2; // Phase of the motion of revolution
262 
263  // Vector of the markers sticked on the object
264  const std::vector<vpColVector> markers = { vpColVector({-0.05, 0.05, 0., 1.}),
265  vpColVector({0.05, 0.05, 0., 1.}),
266  vpColVector({0.05, -0.05, 0., 1.}),
267  vpColVector({-0.05, -0.05, 0., 1.}) };
268  const unsigned int nbMarkers = static_cast<unsigned int>(markers.size());
269  std::vector<vpPoint> markersAsVpPoint;
270  for (unsigned int i = 0; i < nbMarkers; ++i) {
271  vpColVector marker = markers[i];
272  markersAsVpPoint.push_back(vpPoint(marker[0], marker[1], marker[2]));
273  }
274 
275  const long seed = 42; // Seed for the random generator
276  vpHomogeneousMatrix cMw; // Pose of the world frame with regard to the camera frame
277  cMw[0][0] = 1.; cMw[0][1] = 0.; cMw[0][2] = 0.; cMw[0][3] = 0.2;
278  cMw[1][0] = 0.; cMw[1][1] = -1.; cMw[1][2] = 0.; cMw[1][3] = 0.3;
279  cMw[2][0] = 0.; cMw[2][1] = 0.; cMw[2][2] = -1.; cMw[2][3] = 1.;
280 
281  vpHomogeneousMatrix wMo; // Pose of the object frame with regard to the world frame
282  wMo[0][0] = 1.; wMo[0][1] = 0.; wMo[0][2] = 0.; wMo[0][3] = radius;
283  wMo[1][0] = 0.; wMo[1][1] = 1.; wMo[1][2] = 0.; wMo[1][3] = 0;
284  wMo[2][0] = 0.; wMo[2][1] = 0.; wMo[2][2] = 1.; wMo[2][3] = 0.2;
285  vpRotationMatrix wRo; // Rotation between the object frame and world frame
286  wMo.extract(wRo);
287  const double wZ = wMo[2][3];
289 
291  // Create a camera parameter container
292  // Camera initialization with a perspective projection without distortion model
293  double px = 600; double py = 600; double u0 = 320; double v0 = 240;
294  vpCameraParameters cam;
295  cam.initPersProjWithoutDistortion(px, py, u0, v0);
297 
298  // Initialize the attributes of the UKF
300  std::shared_ptr<vpUKSigmaDrawerAbstract> drawer = std::make_shared<vpUKSigmaDrawerMerwe>(4, 0.001, 2., -1);
302 
304  vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark
305  R1landmark[0][0] = sigmaMeasurements*sigmaMeasurements;
306  R1landmark[1][1] = sigmaMeasurements*sigmaMeasurements;
307  vpMatrix R(2*nbMarkers, 2 * nbMarkers);
308  for (unsigned int i = 0; i < nbMarkers; ++i) {
309  R.insert(R1landmark, 2*i, 2*i);
310  }
312 
314  const double processVariance = 0.000025; // Variance of the process of (0.005cm)^2
315  vpMatrix Q; // The covariance of the process
316  Q.eye(4);
317  Q = Q * processVariance;
319 
321  vpMatrix P0(4, 4); // The initial guess of the process covariance
322  P0.eye(4);
323  P0[0][0] = 1.;
324  P0[1][1] = 1.;
325  P0[2][2] = 1.;
326  P0[2][2] = 5.;
327 
328  vpColVector X0(4); // The initial guess for the state
329  X0[0] = radius; // wX = radius m
330  X0[1] = 0.; // wY = 0m
331  X0[2] = 0.95 * wZ; // Wrong estimation of the position along the z-axis: error of 5%
332  X0[3] = 0.75 * w * dt; // Wrong estimation of the pulsation: error of 25%
334 
337  vpMarkersMeasurements markerMeas(cam, cMw, wRo, markers, sigmaMeasurements, seed);
338  using std::placeholders::_1;
341 
343  // Initialize the UKF
344  vpUnscentedKalman ukf(Q, R, drawer, f, h);
345  ukf.init(X0, P0);
347 
349 #ifdef VISP_HAVE_DISPLAY
350  // Initialize the plot
351  vpPlot plot(1);
352  plot.initGraph(0, 3);
353  plot.setTitle(0, "Position of the robot wX");
354  plot.setUnitX(0, "Position along x(m)");
355  plot.setUnitY(0, "Position along y (m)");
356  plot.setLegend(0, 0, "GT");
357  plot.setLegend(0, 1, "Filtered");
358  plot.setLegend(0, 2, "Measure");
359  plot.initRange(0, -1.25 * radius, 1.25 * radius, -1.25 * radius, 1.25 * radius);
360  plot.setColor(0, 0, vpColor::red);
361  plot.setColor(0, 1, vpColor::blue);
362  plot.setColor(0, 2, vpColor::black);
363 #endif
365 
367  // Initialize the display
368  // Depending on the detected third party libraries, we instantiate here the
369  // first video device which is available
370 #ifdef VISP_HAVE_DISPLAY
371  vpImage<vpRGBa> Idisp(700, 700, vpRGBa(255));
372  std::shared_ptr<vpDisplay> d = vpDisplayFactory::createDisplay(Idisp, 800, -1, "Projection of the markers");
373 #endif
375 
377  // Initialize the simulation
378  vpObjectSimulator object(radius, w, phi, wZ);
379  vpColVector object_pos(4, 0.);
380  object_pos[3] = 1.;
382 
384  for (unsigned int i = 0; i < 200; ++i) {
386  // Update object pose
387  object_pos = object.move(dt * static_cast<double>(i));
389 
391  // Perform the measurement
392  vpColVector z = markerMeas.measureWithNoise(object_pos);
394 
396  // Use the UKF to filter the measurement
397  ukf.filter(z, dt);
399 
401 #ifdef VISP_HAVE_DISPLAY
403  // Prepare the pose computation:
404  // the image points corresponding to the noisy markers are needed
405  std::vector<vpImagePoint> ip;
406  for (unsigned int id = 0; id < nbMarkers; ++id) {
407  vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
408  ip.push_back(markerProjNoisy);
409  }
410 
411  // Compute the pose using the noisy markers
412  vpHomogeneousMatrix cMo_noisy = computePose(markersAsVpPoint, ip, cam);
413  vpHomogeneousMatrix wMo_noisy = cMw.inverse() * cMo_noisy;
414  double wXnoisy = wMo_noisy[0][3];
415  double wYnoisy = wMo_noisy[1][3];
417 
419  // Plot the ground truth
420  plot.plot(0, 0, object_pos[0], object_pos[1]);
421 
422  // Plot the filtered state
423  vpColVector Xest = ukf.getXest();
424  plot.plot(0, 1, Xest[0], Xest[1]);
425 
426  // Plot the noisy pose
427  plot.plot(0, 2, wXnoisy, wYnoisy);
429 
431  // Display the projection of the markers
432  vpDisplay::display(Idisp);
433  vpColVector zGT = markerMeas.measureGT(object_pos);
434  vpColVector zFilt = markerMeas.state_to_measurement(Xest);
435  for (unsigned int id = 0; id < nbMarkers; ++id) {
436  vpImagePoint markerProjGT(zGT[2*id + 1], zGT[2*id]);
437  vpDisplay::displayCross(Idisp, markerProjGT, 5, vpColor::red);
438 
439  vpImagePoint markerProjFilt(zFilt[2*id + 1], zFilt[2*id]);
440  vpDisplay::displayCross(Idisp, markerProjFilt, 5, vpColor::blue);
441 
442  vpImagePoint markerProjNoisy(z[2*id + 1], z[2*id]);
443  vpDisplay::displayCross(Idisp, markerProjNoisy, 5, vpColor::black);
444  }
445 
446  vpImagePoint ipText(20, 20);
447  vpDisplay::displayText(Idisp, ipText, std::string("GT"), vpColor::red);
448  ipText.set_i(ipText.get_i() + 20);
449  vpDisplay::displayText(Idisp, ipText, std::string("Filtered"), vpColor::blue);
450  ipText.set_i(ipText.get_i() + 20);
451  vpDisplay::displayText(Idisp, ipText, std::string("Measured"), vpColor::black);
452  vpDisplay::flush(Idisp);
453  vpTime::wait(40);
455 #endif
457  }
459  std::cout << "Press Enter to quit..." << std::endl;
460  std::cin.get();
461 
462  return 0;
463 }
464 #else
465 int main()
466 {
467  std::cout << "This example is only available if you compile ViSP in C++11 standard or higher." << std::endl;
468  return 0;
469 }
470 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
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:191
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor black
Definition: vpColor.h:211
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Class for generating random number with normal probability density.
Definition: vpGaussRand.h:117
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
[Object_simulator]
vpColVector state_to_measurement(const vpColVector &x)
[Measurement_function]
vpMarkersMeasurements(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMw, const vpRotationMatrix &wRo, const std::vector< vpColVector > &markers, const double &noise_stdev, const long &seed)
Construct a new vpMarkersMeasurements object.
vpColVector measureGT(const vpColVector &wX)
[Measurement_function]
vpColVector measureWithNoise(const vpColVector &wX)
[GT_measurements]
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
[Process_function]
vpColVector move(const double &t) const
Move the object to its new position, expressed in the world frame.
vpObjectSimulator(const double &R, const double &w, const double &phi, const double &wZ)
Construct a new vpObjectSimulator object.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:77
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:96
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:98
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
Definition: vpPose.cpp:385
Definition: vpRGBa.h:65
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.
std::function< vpColVector(const vpColVector &)> vpMeasurementFunction
Measurement function, which converts the prior points in the measurement space. The argument is a poi...
std::function< vpColVector(const vpColVector &, const double &)> vpProcessFunction
Process model function, which projects the sigma points forward in time. The first argument is a sigm...
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT int wait(double t0, double t)