#include "ClassUsingPclViewer.h"
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
#include <pcl/io/pcd_io.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpGaussRand.h>
#include <visp3/core/vpRobust.h>
#include <visp3/gui/vpColorBlindFriendlyPalette.h>
#include <visp3/io/vpKeyboard.h>
double zFunction(const double &x, const double &y, const unsigned int order)
{
const double offset(0.5);
double z(0.);
for (unsigned int n = 0; n <= order; n++) {
for (unsigned int k = 0; k <= order - n; k++) {
if (k + n > 0) {
z += std::pow(x, n) * std::pow(y, k);
}
else {
z += offset;
}
}
}
return z;
}
ClassUsingPclViewer::ClassUsingPclViewer(std::pair<double, double> xlimits, std::pair<double, double> ylimits, std::pair<unsigned int, unsigned int> nbPoints)
: m_t(0.1, 0.1, 0.1)
, m_R(M_PI_4, M_PI_4, M_PI_4)
, m_cMo(m_t, m_R)
, m_minX(xlimits.first)
, m_maxX(xlimits.second)
, m_n(nbPoints.first)
, m_minY(ylimits.first)
, m_maxY(ylimits.second)
, m_m(nbPoints.second)
, m_visualizer("Grid of points with / without robust")
{
m_dX = (m_maxX - m_minX) / (static_cast<double>(m_n) - 1.);
m_dY = (m_maxY - m_minY) / (static_cast<double>(m_m) - 1.);
}
ClassUsingPclViewer::~ClassUsingPclViewer()
{
}
std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> ClassUsingPclViewer::generateControlPoints(
const double &addedNoise,
const unsigned int &order,
vpColVector &confidenceWeights)
{
std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> result;
vpPclViewer::pclPointCloudPointXYZRGBPtr unrotatedControlPoints(new vpPclViewer::pclPointCloudPointXYZRGB(m_n, m_m));
vpPclViewer::pclPointCloudPointXYZRGBPtr rotatedControlPoints(new vpPclViewer::pclPointCloudPointXYZRGB(m_n, m_m));
confidenceWeights.
resize(m_m * m_n);
for (unsigned int j = 0; j < m_m; j++) {
for (unsigned int i = 0; i < m_n; i++) {
double oX = m_minX + (double)i * m_dX;
double oY = m_minY + (double)j * m_dY;
double oZ = zFunction(oX, oY, order);
std::vector<double> point = { oX, oY, oZ,1. };
(*unrotatedControlPoints)(i, j).x = oCoords[0];
(*unrotatedControlPoints)(i, j).y = oCoords[1];
(*unrotatedControlPoints)(i, j).z = oCoords[2];
(*rotatedControlPoints)(i, j).x = cCoords[0];
(*rotatedControlPoints)(i, j).y = cCoords[1];
double noise = r();
(*rotatedControlPoints)(i, j).z = cCoords[2] + noise;
confidenceWeights[j * m_n + i] = 1.;
residuals[j * m_n + i] = noise;
}
}
if (std::abs(addedNoise) > 0.) {
}
result.first = unrotatedControlPoints;
result.second = rotatedControlPoints;
return result;
}
void ClassUsingPclViewer::blockingMode(const double &addedNoise, const unsigned int &order)
{
std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> grids = generateControlPoints(addedNoise, order, confWeights);
unsigned int id_ctrlPts = m_visualizer.addSurface(grids.first, "Standard");
unsigned int id_robust = m_visualizer.addSurface(grids.second, confWeights, "RotatedWithRobust", color.to_RGB());
std::cout << "Press \"q\" while selecting the viewer window to stop the program." << std::endl;
m_visualizer.display();
(void)id_ctrlPts;
(void)id_robust;
}
void ClassUsingPclViewer::threadedMode(const double &addedNoise, const unsigned int &order)
{
std::pair<vpPclViewer::pclPointCloudPointXYZRGBPtr, vpPclViewer::pclPointCloudPointXYZRGBPtr> grids = generateControlPoints(addedNoise, order, confWeights);
unsigned int id_ctrlPts = m_visualizer.addSurface(grids.first, "Standard");
unsigned int id_robust = m_visualizer.addSurface(grids.second, confWeights, "RotatedWithRobust", color.to_RGB());
m_visualizer.launchThread();
m_visualizer.updateSurface(grids.first, id_ctrlPts);
m_visualizer.updateSurface(grids.second, id_robust, confWeights);
bool wantToStop = false;
double t;
std::cout << "Press any key in the console to stop the program." << std::endl;
while (!wantToStop) {
grids = generateControlPoints(addedNoise, order, confWeights);
m_visualizer.updateSurface(grids.first, id_ctrlPts);
m_visualizer.updateSurface(grids.second, id_robust, confWeights);
wantToStop = true;
}
}
}
#else
void dummy_class_using_pcl_visualizer()
{ }
#endif
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()