2#include "ClassUsingPclViewer.h"
4#if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
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>
16double 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);
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.);
60std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> ClassUsingPclViewer::generateControlPoints(
const double &addedNoise,
const unsigned int &order,
vpColVector &confidenceWeights)
62 std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> result;
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;
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;
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());
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);
189 m_visualizer.
updateSurface(grids.second, id_robust, confWeights);
192 if (keyboard.
kbhit()) {
200void dummy_class_using_pcl_visualizer()
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)
Keyboard management under unix (Linux or OSX). This class is not available under windows.
void display()
Blocking-mode display of the viewer.
void launchThread()
Start the drawing thread that permits to have a non-blocking display.
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.
pclPointCloudPointXYZRGB::Ptr pclPointCloudPointXYZRGBPtr
pcl::PointCloud< pclPointXYZRGB > pclPointCloudPointXYZRGB
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 double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMicros()