1 #include "ClassUsingPclViewer.h"
4 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
6 #include <pcl/io/pcd_io.h>
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>
16 double zFunction(
const double &x,
const double &y,
const unsigned int order)
18 const double offset(0.5);
21 for (
unsigned int n = 0; n <= order; n++) {
22 for (
unsigned int k = 0; k <= order - n; k++) {
24 z += std::pow(x, n) * std::pow(y, k);
37 ClassUsingPclViewer::ClassUsingPclViewer(std::pair<double, double> xlimits, std::pair<double, double> ylimits, std::pair<unsigned int, unsigned int> nbPoints)
39 , m_R(M_PI_4, M_PI_4, M_PI_4)
41 , m_minX(xlimits.first)
42 , m_maxX(xlimits.second)
44 , m_minY(ylimits.first)
45 , m_maxY(ylimits.second)
46 , m_m(nbPoints.second)
47 , m_visualizer(
"Grid of points with / without robust")
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.);
54 ClassUsingPclViewer::~ClassUsingPclViewer()
60 std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> ClassUsingPclViewer::generateControlPoints(
const double &addedNoise,
const unsigned int &order,
vpColVector &confidenceWeights)
62 std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> result;
65 vpPclViewer::pclPointCloudPointXYZRGBPtr unrotatedControlPoints(
new vpPclViewer::pclPointCloudPointXYZRGB(m_n, m_m));
66 vpPclViewer::pclPointCloudPointXYZRGBPtr rotatedControlPoints(
new vpPclViewer::pclPointCloudPointXYZRGB(m_n, m_m));
69 confidenceWeights.
resize(m_m * m_n);
80 for (
unsigned int j = 0; j < m_m; j++) {
81 for (
unsigned int i = 0; i < m_n; i++) {
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);
89 std::vector<double> point = { oX, oY, oZ,1. };
91 (*unrotatedControlPoints)(i, j).x = oCoords[0];
92 (*unrotatedControlPoints)(i, j).y = oCoords[1];
93 (*unrotatedControlPoints)(i, j).z = oCoords[2];
97 (*rotatedControlPoints)(i, j).x = cCoords[0];
98 (*rotatedControlPoints)(i, j).y = cCoords[1];
102 (*rotatedControlPoints)(i, j).z = cCoords[2] + noise;
105 confidenceWeights[j * m_n + i] = 1.;
109 residuals[j * m_n + i] = noise;
113 if (std::abs(addedNoise) > 0.) {
121 result.first = unrotatedControlPoints;
122 result.second = rotatedControlPoints;
127 void ClassUsingPclViewer::blockingMode(
const double &addedNoise,
const unsigned int &order)
133 std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> grids = generateControlPoints(addedNoise, order, confWeights);
138 unsigned int id_ctrlPts = m_visualizer.addSurface(grids.first,
"Standard");
144 unsigned int id_robust = m_visualizer.addSurface(grids.second, confWeights,
"RotatedWithRobust", color.to_RGB());
147 std::cout <<
"Press \"q\" while selecting the viewer window to stop the program." << std::endl;
149 m_visualizer.display();
156 void ClassUsingPclViewer::threadedMode(
const double &addedNoise,
const unsigned int &order)
162 std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> grids = generateControlPoints(addedNoise, order, confWeights);
165 unsigned int id_ctrlPts = m_visualizer.addSurface(grids.first,
"Standard");
169 unsigned int id_robust = m_visualizer.addSurface(grids.second, confWeights,
"RotatedWithRobust", color.to_RGB());
172 m_visualizer.launchThread();
175 m_visualizer.updateSurface(grids.first, id_ctrlPts);
176 m_visualizer.updateSurface(grids.second, id_robust, confWeights);
179 bool wantToStop =
false;
182 std::cout <<
"Press any key in the console to stop the program." << std::endl;
183 while (!wantToStop) {
185 grids = generateControlPoints(addedNoise, order, confWeights);
188 m_visualizer.updateSurface(grids.first, id_ctrlPts);
189 m_visualizer.updateSurface(grids.second, id_robust, confWeights);
192 if (keyboard.
kbhit()) {
200 void dummy_class_using_pcl_visualizer()
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...
Class for generating random number with normal probability density.
void setSigmaMean(double sigma_val, double mean_val)
Keyboard management under unix (Linux or OSX). This class is not available under windows.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
void setMinMedianAbsoluteDeviation(double mad_min)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()