Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
ClassUsingPclViewer.cpp
1
2#include "ClassUsingPclViewer.h"
3
4#if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
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
16double 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
37ClassUsingPclViewer::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
58
60std::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
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
127void 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
143 vpColorBlindFriendlyPalette color(vpColorBlindFriendlyPalette::Palette::Purple);
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
156void 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
168 vpColorBlindFriendlyPalette color(vpColorBlindFriendlyPalette::Palette::Purple);
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) {
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
200void 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.
void resize(unsigned int i, bool flagNullify=true)
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.
void setSigmaMean(double sigma_val, double mean_val)
void seed(long seed_val)
Keyboard management under unix (Linux or OSX). This class is not available under windows.
Definition vpKeyboard.h:82
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:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()