Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
ClassUsingPclViewer.cpp
1 #include "ClassUsingPclViewer.h"
3 
4 #if defined(VISP_HAVE_PCL)
5 // PCL
6 #include <pcl/io/pcd_io.h>
7 
8 // Visp
9 #include <visp3/core/vpTime.h>
10 #include <visp3/core/vpGaussRand.h>
11 #include <visp3/core/vpRobust.h>
12 #include <visp3/gui/vpColorBlindFriendlyPalette.h>
13 #include <visp3/io/vpKeyboard.h>
14 
16 double zFunction(const double &x, const double &y, const unsigned int order)
17 {
18  const double offset(0.5);
19  double z(0.);
20 
21  for (unsigned int n = 0; n <= order; n++) {
22  for (unsigned int k = 0; k <= order - n; k++) {
23  if (k + n > 0) {
24  z += std::pow(x, n) * std::pow(y, k);
25  }
26  else {
27  z += offset;
28  }
29  }
30  }
31 
32  return z;
33 }
35 
37 ClassUsingPclViewer::ClassUsingPclViewer(std::pair<double, double> xlimits, std::pair<double, double> ylimits, std::pair<unsigned int, unsigned int> nbPoints)
38  : m_t(0.1, 0.1, 0.1)
39  , m_R(M_PI_4, M_PI_4, M_PI_4)
40  , m_cMo(m_t, m_R)
41  , m_minX(xlimits.first)
42  , m_maxX(xlimits.second)
43  , m_n(nbPoints.first)
44  , m_minY(ylimits.first)
45  , m_maxY(ylimits.second)
46  , m_m(nbPoints.second)
47  , m_visualizer("Grid of points with / without robust")
48 {
49  m_dX = (m_maxX - m_minX) / (static_cast<double>(m_n) - 1.);
50  m_dY = (m_maxY - m_minY) / (static_cast<double>(m_m) - 1.);
51 }
53 
55 {
56 
57 }
58 
60 std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> ClassUsingPclViewer::generateControlPoints(const double &addedNoise, const unsigned int &order, vpColVector &confidenceWeights)
61 {
62  std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> result;
63 
64  // Create control points
67 
68  // Initializing confindence weights
69  confidenceWeights.resize(m_m * m_n);
70 
71  // Noise generator for the observed points
72  // We deriberately choose to set the standard deviation to be twice the tolerated value to observe rejected points
73  vpGaussRand r;
74  r.setSigmaMean(addedNoise * 2., 0.);
76 
77  // Residual vector to compute the confidence weights
78  vpColVector residuals(m_m * m_n);
79 
80  for (unsigned int j = 0; j < m_m; j++) {
81  for (unsigned int i = 0; i < m_n; i++) {
82  // Creating model, expressed in the object frame
83  double oX = m_minX + (double)i * m_dX;
84  double oY = m_minY + (double)j * m_dY;
85  double oZ = zFunction(oX, oY, order);
86 
87  // Setting the point coordinates of the first point cloud in
88  // the object frame
89  std::vector<double> point = { oX, oY, oZ,1. };
90  vpColVector oCoords = vpColVector(point);
91  (*unrotatedControlPoints)(i, j).x = oCoords[0];
92  (*unrotatedControlPoints)(i, j).y = oCoords[1];
93  (*unrotatedControlPoints)(i, j).z = oCoords[2];
94 
95  // Moving the point into another coordinate frame
96  vpColVector cCoords = m_cMo * oCoords;
97  (*rotatedControlPoints)(i, j).x = cCoords[0];
98  (*rotatedControlPoints)(i, j).y = cCoords[1];
99 
100  // Potentially adding some noise if the user asked to
101  double noise = r();
102  (*rotatedControlPoints)(i, j).z = cCoords[2] + noise;
103 
104  // Filling the confidence weights with default value of 1.
105  confidenceWeights[j * m_n + i] = 1.;
106 
107  // Indicating the residual, here it corresponds to the difference
108  // between the theoretical position of the point and the actual one
109  residuals[j * m_n + i] = noise;
110  }
111  }
112 
113  if (std::abs(addedNoise) > 0.) {
114  // Estimating the confidence weights to remove points suffering too much noise
115  // See vpRobust documentation for more information.
116  vpRobust robust;
117  robust.setMinMedianAbsoluteDeviation(addedNoise);
118  robust.MEstimator(vpRobust::TUKEY, residuals, confidenceWeights);
119  }
120 
121  result.first = unrotatedControlPoints;
122  result.second = rotatedControlPoints;
123  return result;
124 }
126 
127 void ClassUsingPclViewer::blockingMode(const double &addedNoise, const unsigned int &order)
128 {
129  // Confidence weights, that would be obtained thanks to vpRobust for instance
130  vpColVector confWeights;
131 
133  std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> grids = generateControlPoints(addedNoise, order, confWeights);
135 
137  // Adding a point cloud for which we don't chose the color
138  unsigned int id_ctrlPts = m_visualizer.addSurface(grids.first, "Standard");
140 
142  // Adding a point cloud for which we chose the color
144  unsigned int id_robust = m_visualizer.addSurface(grids.second, confWeights, "RotatedWithRobust", color.to_RGB());
146 
147  std::cout << "Press \"q\" while selecting the viewer window to stop the program." << std::endl;
149  m_visualizer.display();
151 
152  (void)id_ctrlPts;
153  (void)id_robust;
154 }
155 
156 void ClassUsingPclViewer::threadedMode(const double &addedNoise, const unsigned int &order)
157 {
158  // Confidence weights, that would be obtained thanks to vpRobust for instance
159  vpColVector confWeights;
160 
161  // Create control points
162  std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> grids = generateControlPoints(addedNoise, order, confWeights);
163 
164  // Adding a point cloud for which we don't chose the color
165  unsigned int id_ctrlPts = m_visualizer.addSurface(grids.first, "Standard");
166 
167  // Adding a point cloud for which we chose the color
169  unsigned int id_robust = m_visualizer.addSurface(grids.second, confWeights, "RotatedWithRobust", color.to_RGB());
170 
172  m_visualizer.launchThread();
174 
175  m_visualizer.updateSurface(grids.first, id_ctrlPts);
176  m_visualizer.updateSurface(grids.second, id_robust, confWeights);
177 
178  vpKeyboard keyboard;
179  bool wantToStop = false;
180  double t;
181 
182  std::cout << "Press any key in the console to stop the program." << std::endl;
183  while (!wantToStop) {
184  t = vpTime::measureTimeMs();
185  grids = generateControlPoints(addedNoise, order, confWeights);
186 
188  m_visualizer.updateSurface(grids.first, id_ctrlPts);
189  m_visualizer.updateSurface(grids.second, id_robust, confWeights);
191 
192  if (keyboard.kbhit()) {
193  wantToStop = true;
194  }
195  vpTime::wait(t, 40);
196  }
197 
198 }
199 #else
200 void dummy_class_using_pcl_visualizer()
201 { }
202 #endif
void threadedMode(const double &addedNoise, const unsigned int &order)
Demonstration on how to use a vpPclViewer in threaded mode.
~ClassUsingPclViewer()
[Constructor]
ClassUsingPclViewer(std::pair< double, double > xlimits={ -2.5, 2.5 }, std::pair< double, double > ylimits={ -2.5, 2.5 }, std::pair< unsigned int, unsigned int > nbPoints={ 50, 50 })
Construct a new object.
void blockingMode(const double &addedNoise, const unsigned int &order)
Demonstration on how to use a vpPclViewer in blocking mode, i.e. we expect an input from the user aft...
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class that furnishes a set of colors that color blind people should be able to distinguish one from a...
std::vector< unsigned char > to_RGB() const
Cast a vpColorBlindFriendlyPalette in a vector {R, G, B}. A vpColorBlindFriendlyPalette::Palette::COU...
Class for generating random number with normal probability density.
Definition: vpGaussRand.h:116
void setSigmaMean(double sigma_val, double mean_val)
Definition: vpGaussRand.h:150
void seed(long seed_val)
Definition: vpGaussRand.h:161
Keyboard management under unix (Linux or OSX). This class is not available under windows.
Definition: vpKeyboard.h:82
int kbhit()
Definition: vpKeyboard.cpp:96
void display()
Blocking-mode display of the viewer.
pcl::PointCloud< pclPointXYZRGB > pclPointCloudPointXYZRGB
Definition: vpPclViewer.h:71
void launchThread()
Start the drawing thread that permits to have a non-blocking display.
pclPointCloudPointXYZRGB::Ptr pclPointCloudPointXYZRGBPtr
Definition: vpPclViewer.h:72
void updateSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id, const bool &hasToKeepColor=false)
Update the surface known by id by the viewer.
unsigned int addSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const std::string &name="", const std::vector< unsigned char > &v_color=std::vector< unsigned char >())
Add a surface to the list of point clouds known by the viewer.
Contains an M-estimator and various influence function.
Definition: vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:136
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:156
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()