35 #ifndef DOXYGEN_SHOULD_SKIP_THIS
37 #include <visp3/core/vpConfig.h>
38 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_THREADS)
40 #include<pcl/io/pcd_io.h>
43 #include <visp3/gui/vpPclViewer.h>
44 #include <visp3/gui/vpColorBlindFriendlyPalette.h>
45 #include <visp3/core/vpIoTools.h>
51 const unsigned int gc_nbColorMax = 7;
53 pcl::visualization::PCLVisualizer::Ptr vpPclViewer::sp_viewer(
nullptr);
55 std::vector<std::vector<double>> vpPclViewer::s_vhandler;
57 int vpPclViewer::s_width = 640;
58 int vpPclViewer::s_height = 480;
59 int vpPclViewer::s_posU = 40;
60 int vpPclViewer::s_posV = 40;
61 double vpPclViewer::s_ignoreThresh = 0.95;
63 vpPclViewer::vpPclViewer(
const std::string &title,
const int &width,
const int &height
64 ,
const int &posU,
const int &posV
65 ,
const std::string &outFolder,
const double &ignoreThreshold)
68 , m_hasToSavePCDs(false)
69 , m_outFolder(outFolder)
71 setOutFolder(outFolder);
72 setIgnoreThreshold(ignoreThreshold);
79 vpPclViewer ::~vpPclViewer()
85 for (
unsigned int i = 0; i < m_vPointClouds.size(); i++) {
86 m_vPointClouds[i].reset();
88 m_vPointClouds.clear();
91 for (
unsigned int id = 0;
id < m_vpmutex.size();
id++) {
102 void vpPclViewer::setNameWindow(
const std::string &nameWindow)
104 sp_viewer->setWindowName(nameWindow);
107 void vpPclViewer::setOutFolder(
const std::string &outFolder)
109 m_outFolder = outFolder;
110 if (!m_outFolder.empty()) {
112 m_hasToSavePCDs =
true;
120 m_hasToSavePCDs =
false;
124 void vpPclViewer::setIgnoreThreshold(
const double &ignoreThreshold)
126 if (ignoreThreshold < 0. || ignoreThreshold > 1.) {
129 s_ignoreThresh = ignoreThreshold;
132 void vpPclViewer::updateSurface(
const pclPointCloudPointXYZRGB::Ptr &surface,
const unsigned int &
id,
const bool &hasToKeepColor)
136 if (hasToKeepColor) {
138 threadUpdateSurfaceOriginalColor(surface,
id);
142 threadUpdateSurface(surface,
id);
148 updateSurface(surface,
id, fakeWeights, hasToKeepColor);
152 void vpPclViewer::updateSurface(
const pclPointCloudPointXYZRGB::Ptr &surface,
const unsigned int &
id,
153 const vpColVector &weights,
const bool &hasToKeepColor)
157 if (hasToKeepColor) {
159 threadUpdateSurfaceOriginalColor(surface,
id, weights);
163 threadUpdateSurface(surface,
id, weights);
169 if (!m_vPointClouds[
id]) {
170 m_vPointClouds[id].reset(
new pclPointCloudPointXYZRGB());
174 unsigned int nbPoints = surface->size();
175 m_vPointClouds[id]->resize(nbPoints);
178 m_vweights[id] = weights;
179 bool use_weights = (weights.
size() > 0);
182 for (
unsigned int index = 0; index < nbPoints; index++) {
183 bool addPoint =
false;
185 addPoint = (weights[index] > s_ignoreThresh);
191 pclPointXYZRGB pt = surface->at(index);
193 m_vPointClouds[id]->at(index).x = pt.x;
194 m_vPointClouds[id]->at(index).y = pt.y;
195 m_vPointClouds[id]->at(index).z = pt.z;
197 m_vPointClouds[id]->at(index).r = s_vhandler[id][0];
198 m_vPointClouds[id]->at(index).g = s_vhandler[id][1];
199 m_vPointClouds[id]->at(index).b = s_vhandler[id][2];
205 bool status = sp_viewer->updatePointCloud(m_vPointClouds[
id], m_vmeshid[
id]);
207 std::stringstream err_msg;
208 err_msg <<
"[vpPclViewer ::updateSurface] ID " << m_vmeshid[id] <<
" not found !" << std::endl;
214 unsigned int vpPclViewer::addSurface(
const pclPointCloudPointXYZRGB::Ptr &surface,
const std::string &name,
const std::vector<unsigned char> &v_color)
217 return addSurface(surface, emptyWeights, name, v_color);
220 unsigned int vpPclViewer::addSurface(
const pclPointCloudPointXYZRGB::Ptr &surface,
const vpColVector &weights,
const std::string &name,
const std::vector<unsigned char> &v_color)
222 static unsigned int nbSurfaces = 0;
223 unsigned int id = m_vPointClouds.size();
226 pclPointCloudPointXYZRGB::Ptr p_pointCloud(
new pclPointCloudPointXYZRGB());
227 m_vPointClouds.push_back(p_pointCloud);
230 unsigned int nbPoints = surface->size();
231 m_vPointClouds[id]->resize(nbPoints);
234 std::vector<unsigned char> v_RGB;
235 if (v_color.size() < 3) {
239 v_RGB = color.to_RGB();
246 std::vector<double> v_RGBdouble = {
251 s_vhandler.push_back(v_RGBdouble);
254 m_vweights.push_back(weights);
255 bool use_weigths = weights.
size() > 0;
260 for (
unsigned int index = 0; index < nbPoints; index++) {
261 bool shouldPointBeVisible =
false;
265 shouldPointBeVisible = weights[index] > s_ignoreThresh;
269 shouldPointBeVisible =
true;
272 pclPointXYZRGB pt = surface->at(index);
273 m_vPointClouds[id]->at(index).x = pt.x;
274 m_vPointClouds[id]->at(index).y = pt.y;
275 m_vPointClouds[id]->at(index).z = pt.z;
277 if (shouldPointBeVisible) {
278 m_vPointClouds[id]->at(index).r = s_vhandler[id][0];
279 m_vPointClouds[id]->at(index).g = s_vhandler[id][1];
280 m_vPointClouds[id]->at(index).b = s_vhandler[id][2];
283 m_vPointClouds[id]->at(index).r = 0.;
284 m_vPointClouds[id]->at(index).g = 0.;
285 m_vPointClouds[id]->at(index).b = 0.;
296 m_vmeshid.push_back(name);
300 m_vmeshid.push_back(
"point_cloud" + std::to_string(
id));
305 sp_viewer->addPointCloud(m_vPointClouds[
id], m_vmeshid[
id]);
309 nbSurfaces = (nbSurfaces + 1) % gc_nbColorMax;
312 m_vpmutex.push_back(
new std::mutex());
316 m_vlegends.push_back(legend);
317 legendParams &newLegend = m_vlegends[id];
318 newLegend.m_text = m_vmeshid[id];
319 newLegend.m_posU = 10;
320 newLegend.m_posV = 10;
321 newLegend.m_size = 16;
323 newLegend.m_posV = m_vlegends[
id - 1].m_posV + newLegend.m_size;
325 newLegend.m_rRatio = s_vhandler[id][0] / 255.0;
326 newLegend.m_gRatio = s_vhandler[id][1] / 255.0;
327 newLegend.m_bRatio = s_vhandler[id][2] / 255.0;
331 sp_viewer->addText(newLegend.m_text, newLegend.m_posU, newLegend.m_posV, newLegend.m_rRatio, newLegend.m_gRatio, newLegend.m_bRatio);
339 void vpPclViewer::display()
344 sp_viewer.reset(
new pcl::visualization::PCLVisualizer(m_title));
345 sp_viewer->addCoordinateSystem(0.5);
346 sp_viewer->initCameraParameters();
347 sp_viewer->setSize(s_width, s_height);
348 sp_viewer->setPosition(s_posU, s_posV);
349 sp_viewer->resetCamera();
351 for (
unsigned int id = 0;
id < m_vPointClouds.size();
id++) {
352 sp_viewer->addPointCloud(m_vPointClouds[
id], m_vmeshid[
id]);
353 sp_viewer->addText(m_vlegends[
id].m_text, m_vlegends[
id].m_posU, m_vlegends[
id].m_posV, m_vlegends[
id].m_rRatio, m_vlegends[
id].m_gRatio, m_vlegends[
id].m_bRatio);
359 void vpPclViewer::refresh(
const int &timeout,
const bool &waitForDrawing)
361 sp_viewer->spinOnce(timeout, waitForDrawing);
364 void vpPclViewer::launchThread()
370 m_threadDisplay = std::thread(vpPclViewer::runThread,
this);
374 void vpPclViewer::stopThread()
380 m_threadDisplay.join();
384 void vpPclViewer::runThread(vpPclViewer *p_visualizer)
386 p_visualizer->loopThread();
389 void vpPclViewer::loopThread()
392 sp_viewer.reset(
new pcl::visualization::PCLVisualizer(m_title));
393 sp_viewer->addCoordinateSystem(0.5);
394 sp_viewer->initCameraParameters();
395 sp_viewer->setSize(s_width, s_height);
396 sp_viewer->setPosition(s_posU, s_posV);
397 sp_viewer->resetCamera();
398 unsigned int iter = 0;
402 for (
unsigned int id = 0;
id < m_vmeshid.size();
id++) {
403 m_vpmutex[id]->lock();
404 unsigned int nbPoints = m_vPointClouds[id]->size();
406 unsigned int nbWeights = m_vweights[id].size();
407 useWeights = (nbWeights > 0);
410 for (
unsigned int index = 0; index < nbPoints; index++) {
411 if (m_vweights[
id][index] < s_ignoreThresh) {
412 m_vPointClouds[id]->at(index).r = 0.0;
413 m_vPointClouds[id]->at(index).g = 0.0;
414 m_vPointClouds[id]->at(index).b = 0.0;
421 if (!sp_viewer->updatePointCloud(m_vPointClouds[
id], m_vmeshid[
id])) {
423 sp_viewer->addPointCloud(m_vPointClouds[
id], m_vmeshid[
id]);
424 sp_viewer->addText(m_vlegends[
id].m_text, m_vlegends[
id].m_posU, m_vlegends[
id].m_posV, m_vlegends[
id].m_rRatio, m_vlegends[
id].m_gRatio, m_vlegends[
id].m_bRatio);
429 if (m_vPointClouds[
id]->size() > 0 && m_hasToSavePCDs) {
431 pcl::io::savePCDFile(filename, *m_vPointClouds[
id]);
434 m_vpmutex[id]->unlock();
436 sp_viewer->spinOnce(100,
true);
444 void vpPclViewer::threadUpdateSurface(
const pclPointCloudPointXYZRGB::Ptr &surface,
const unsigned int &
id)
449 void vpPclViewer::threadUpdateSurfaceOriginalColor(
const pclPointCloudPointXYZRGB::Ptr &surface,
const unsigned int &
id)
451 threadUpdateSurfaceOriginalColor(surface,
id,
vpColVector());
454 void vpPclViewer::threadUpdateSurface(
const pclPointCloudPointXYZRGB::Ptr &surface,
const unsigned int &
id,
const vpColVector &weights)
456 m_vpmutex[id]->lock();
457 m_vweights[id] = weights;
458 unsigned int nbPoints = surface->
size();
459 m_vPointClouds[id]->resize(nbPoints);
463 for (
unsigned int index = 0; index < nbPoints; index++) {
464 pclPointXYZRGB pt = surface->at(index);
465 m_vPointClouds[id]->at(index).x = pt.x;
466 m_vPointClouds[id]->at(index).y = pt.y;
467 m_vPointClouds[id]->at(index).z = pt.z;
469 m_vPointClouds[id]->at(index).r = s_vhandler[id][0];
470 m_vPointClouds[id]->at(index).g = s_vhandler[id][1];
471 m_vPointClouds[id]->at(index).b = s_vhandler[id][2];
473 m_vpmutex[id]->unlock();
476 void vpPclViewer::threadUpdateSurfaceOriginalColor(
const pclPointCloudPointXYZRGB::Ptr &surface,
const unsigned int &
id,
const vpColVector &weights)
478 m_vpmutex[id]->lock();
479 m_vweights[id] = weights;
480 unsigned int nbPoints = surface->
size();
481 m_vPointClouds[id]->resize(nbPoints);
484 pcl::copyPointCloud(*surface, *m_vPointClouds[
id]);
486 m_vpmutex[id]->unlock();
489 #elif !defined(VISP_BUILD_SHARED_LIBS)
491 void dummy_vpPCLPointCLoudVisualization() { };
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
Class that furnishes a set of colors that color blind people should be able to distinguish one from a...
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ notInitialized
Used to indicate that a parameter is not initialized.