Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPclViewer.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 * Description:
31 * Real-time 3D point clouds plotter based on the PCL library.
32 *
33*****************************************************************************/
34
35#ifndef DOXYGEN_SHOULD_SKIP_THIS
36
37#include <visp3/core/vpConfig.h>
38#if defined(VISP_HAVE_PCL) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
39// PCL
40#include<pcl/io/pcd_io.h>
41
42// ViSP
43#include <visp3/gui/vpPclViewer.h>
44#include <visp3/gui/vpColorBlindFriendlyPalette.h>
45#include <visp3/core/vpIoTools.h>
46
47const std::vector<vpColorBlindFriendlyPalette::Palette> gcolor = { vpColorBlindFriendlyPalette::Palette::Green, vpColorBlindFriendlyPalette::Palette::Vermillon,vpColorBlindFriendlyPalette::Palette::Blue,
48 vpColorBlindFriendlyPalette::Palette::Black, vpColorBlindFriendlyPalette::Palette::Orange, vpColorBlindFriendlyPalette::Palette::Purple,
49 vpColorBlindFriendlyPalette::Palette::Yellow };
50
51const unsigned int gc_nbColorMax = 7;
52
53pcl::visualization::PCLVisualizer::Ptr vpPclViewer::sp_viewer(nullptr);
54
55std::vector<std::vector<double>> vpPclViewer::s_vhandler;
56
57int vpPclViewer::s_width = 640;
58int vpPclViewer::s_height = 480;
59int vpPclViewer::s_posU = 40;
60int vpPclViewer::s_posV = 40;
61double vpPclViewer::s_ignoreThresh = 0.95;
62
63vpPclViewer::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)
66 : m_hasToRun(false)
67 , m_title(title)
68 , m_hasToSavePCDs(false)
69 , m_outFolder(outFolder)
70{
71 setOutFolder(outFolder);
72 setIgnoreThreshold(ignoreThreshold);
73 s_width = width;
74 s_height = height;
75 s_posU = posU;
76 s_posV = posV;
77}
78
79vpPclViewer ::~vpPclViewer()
80{
81 // Asking to stop thread
82 stopThread();
83
84 // Deleting point clouds
85 for (unsigned int i = 0; i < m_vPointClouds.size(); i++) {
86 m_vPointClouds[i].reset();
87 }
88 m_vPointClouds.clear();
89
90 // Deleting mutexes
91 for (unsigned int id = 0; id < m_vpmutex.size(); id++) {
92 delete m_vpmutex[id];
93 }
94 m_vpmutex.clear();
95
96 // Deleting the viewer
97 if (sp_viewer) {
98 sp_viewer.reset();
99 }
100}
101
102void vpPclViewer::setNameWindow(const std::string &nameWindow)
103{
104 sp_viewer->setWindowName(nameWindow);
105}
106
107void vpPclViewer::setOutFolder(const std::string &outFolder)
108{
109 m_outFolder = outFolder;
110 if (!m_outFolder.empty()) {
111 // An output folder has been set by the user
112 m_hasToSavePCDs = true;
114 // The output folder does not exist => creating it
116 }
117 }
118 else {
119 // No output folder was assigned to the visualizer
120 m_hasToSavePCDs = false;
121 }
122}
123
124void vpPclViewer::setIgnoreThreshold(const double &ignoreThreshold)
125{
126 if (ignoreThreshold < 0. || ignoreThreshold > 1.) {
127 throw(vpException(vpException::badValue, "[vpPclViewer::setIgnoreThreshold] Fatal error: threshold must be in range [0. ; 1.]"));
128 }
129 s_ignoreThresh = ignoreThreshold;
130}
131
132void vpPclViewer::updateSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id, const bool &hasToKeepColor)
133{
134 if (m_hasToRun) {
135 // Threaded mode
136 if (hasToKeepColor) {
137 // The point cloud will be displayed with its original colors
139 }
140 else {
141 // The point cloud will be displayed with its default colors
142 threadUpdateSurface(surface, id);
143 }
144 }
145 else {
146 // Blocking mode
147 vpColVector fakeWeights; // Fake weights that are all equal to 1, to keep all the points
148 updateSurface(surface, id, fakeWeights, hasToKeepColor);
149 }
150}
151
152void vpPclViewer::updateSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id,
153 const vpColVector &weights, const bool &hasToKeepColor)
154{
155 if (m_hasToRun) {
156 // Threaded mode
157 if (hasToKeepColor) {
158 // The point cloud will be displayed with its original colors
159 threadUpdateSurfaceOriginalColor(surface, id, weights);
160 }
161 else {
162 // The point cloud will be displayed with its default colors
163 threadUpdateSurface(surface, id, weights);
164 }
165 }
166 else {
167 // Blocking mode
168 // If the saved pcl corresponding to \b id was not initialized, initialize it
169 if (!m_vPointClouds[id]) {
171 }
172
173 // Resize if needed the saved pcl corresponding to \b id
174 unsigned int nbPoints = surface->size();
175 m_vPointClouds[id]->resize(nbPoints);
176
177 // Save the new weights and check if they must be used
178 m_vweights[id] = weights;
179 bool use_weights = (weights.size() > 0);
180
181 // Keep only the points that are above \b s_ignoreThresh
182 for (unsigned int index = 0; index < nbPoints; index++) {
183 bool addPoint = false;
184 if (use_weights) {
185 addPoint = (weights[index] > s_ignoreThresh);
186 }
187 else {
188 addPoint = true;
189 }
190
191 pclPointXYZRGB pt = surface->at(index);
192 if (addPoint) {
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;
196
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];
200 }
201 }
202
203 // Try to update the pcl corresponding to the ID
204 // Throw a \b vpException::notInitialized exception if the pcl was not added to the list of known pcl first
205 bool status = sp_viewer->updatePointCloud(m_vPointClouds[id], m_vmeshid[id]);
206 if (!status) {
207 std::stringstream err_msg;
208 err_msg << "[vpPclViewer ::updateSurface] ID " << m_vmeshid[id] << " not found !" << std::endl;
209 throw(vpException(vpException::notInitialized, err_msg.str()));
210 }
211 }
212}
213
214unsigned int vpPclViewer::addSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const std::string &name, const std::vector<unsigned char> &v_color)
215{
216 vpColVector emptyWeights; // Fake weights that are all equal to 1, to keep all the points
217 return addSurface(surface, emptyWeights, name, v_color);
218}
219
220unsigned int vpPclViewer::addSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const vpColVector &weights, const std::string &name, const std::vector<unsigned char> &v_color)
221{
222 static unsigned int nbSurfaces = 0;
223 unsigned int id = m_vPointClouds.size();
224
225 // Creating a new pcl and saving it in the container
226 pclPointCloudPointXYZRGB::Ptr p_pointCloud(new pclPointCloudPointXYZRGB());
227 m_vPointClouds.push_back(p_pointCloud);
228
229 // Sizing it accordingly to the input pcl
230 unsigned int nbPoints = surface->size();
231 m_vPointClouds[id]->resize(nbPoints);
232
233 // Affecting a color to the point cloud and its legend
234 std::vector<unsigned char> v_RGB;
235 if (v_color.size() < 3) {
236 // Affecting a default color to the pcl and its legend, if the user does not want to use its original color
237 // when displaying it
238 vpColorBlindFriendlyPalette color(gcolor[nbSurfaces]);
239 v_RGB = color.to_RGB();
240 }
241 else {
242 // Keeping the colors decided by the user
243 v_RGB = v_color;
244 }
245
246 std::vector<double> v_RGBdouble = {
247 (double)v_RGB[0],
248 (double)v_RGB[1],
249 (double)v_RGB[2]
250 };
251 s_vhandler.push_back(v_RGBdouble);
252
253 // Storing the weights attached to the surface
254 m_vweights.push_back(weights);
255 bool use_weigths = weights.size() > 0;
256
257 // Copying the coordinates of each point of the original pcl,
258 // while affecting them the default color.
259 // Points that have a weight below \b s_ignoreThresh are ignored
260 for (unsigned int index = 0; index < nbPoints; index++) {
261 bool shouldPointBeVisible = false;
262 if (use_weigths) {
263 // If weights are defined, the point should be visible only
264 // if the weight is greater than the ignore threshold.
265 shouldPointBeVisible = weights[index] > s_ignoreThresh;
266 }
267 else {
268 // No weights are used => every points must be considered
269 shouldPointBeVisible = true;
270 }
271
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;
276
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];
281 }
282 else {
283 m_vPointClouds[id]->at(index).r = 0.;
284 m_vPointClouds[id]->at(index).g = 0.;
285 m_vPointClouds[id]->at(index).b = 0.;
286 }
287
288 }
289
290 // std::cout << "Point cloud: width=" << m_vPointClouds[id]->width << ", height= " << m_vPointClouds[id]->height << std::endl;
291
292 // Checking if the user specified a name for the pcl
293 if (!name.empty()) {
294 // Yes => we save it (will be used for the legend, and for
295 // the viewer to know which pcl we are changing when updating pcls)
296 m_vmeshid.push_back(name);
297 }
298 else {
299 // No => we create one, for the very same reasons
300 m_vmeshid.push_back("point_cloud" + std::to_string(id));
301 }
302 // std::cout << "[vpPclViewer ::addSurface] Added ID " << m_vmeshid[id] << " to the list of known point clouds" << std::endl;
303 if (sp_viewer) {
304 // The viewer is already on, we can add the pcl to its known list
305 sp_viewer->addPointCloud(m_vPointClouds[id], m_vmeshid[id]);
306 }
307
308 // Updating the number of known pcls
309 nbSurfaces = (nbSurfaces + 1) % gc_nbColorMax;
310
311 // Adding a mutex to protect the new pcl when using \b threadUpdateSurface
312 m_vpmutex.push_back(new std::mutex());
313
314 // Adding a legend for this pcl
315 legendParams legend;
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;
322 if (id > 0) {
323 newLegend.m_posV = m_vlegends[id - 1].m_posV + newLegend.m_size;
324 }
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;
328
329 if (sp_viewer) {
330 // The viewer is on => we add the legend on the screen
331 sp_viewer->addText(newLegend.m_text, newLegend.m_posU, newLegend.m_posV, newLegend.m_rRatio, newLegend.m_gRatio, newLegend.m_bRatio);
332 }
333
334 return id;
335}
336
337
338
340{
341 stopThread(); // We have to stop the thread to manipulate the viewer with a blocking waiting
342 if (!sp_viewer) {
343 // The viewer was not created => creating a new one
344 sp_viewer.reset(new pcl::visualization::PCLVisualizer(m_title));
345 sp_viewer->addCoordinateSystem(0.5); // Display a coordinate system whose axis are 0.5m long
346 sp_viewer->initCameraParameters(); // Initialize the viewer with default camera settings
347 sp_viewer->setSize(s_width, s_height); // Setting the size of the viewer window
348 sp_viewer->setPosition(s_posU, s_posV); // Setting the position of the viewer window on the screen
349 sp_viewer->resetCamera();
350
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);
354 }
355 }
356 sp_viewer->spin();
357}
358
359void vpPclViewer::refresh(const int &timeout, const bool &waitForDrawing)
360{
361 sp_viewer->spinOnce(timeout, waitForDrawing);
362}
363
365{
366 // Check if the visualization thread is already started
367 if (!m_hasToRun) {
368 // Thread not started => starting it now
369 m_hasToRun = true;
370 m_threadDisplay = std::thread(vpPclViewer::runThread, this);
371 }
372}
373
375{
376 // Check if the visualization thread is running
377 if (m_hasToRun) {
378 // Thread is running, asking it to stop
379 m_hasToRun = false;
380 m_threadDisplay.join();
381 }
382}
383
384void vpPclViewer::runThread(vpPclViewer *p_visualizer)
385{
386 p_visualizer->loopThread();
387}
388
390{
391 bool useWeights;
392 sp_viewer.reset(new pcl::visualization::PCLVisualizer(m_title)); // Allocating a new viewer or resetting the old one.
393 sp_viewer->addCoordinateSystem(0.5); // Display a coordinate system whose axis are 0.5m long
394 sp_viewer->initCameraParameters(); // Initialize the viewer with default camera settings
395 sp_viewer->setSize(s_width, s_height); // Setting the size of the viewer window
396 sp_viewer->setPosition(s_posU, s_posV); // Setting the position of the viewer window on the screen
397 sp_viewer->resetCamera();
398 unsigned int iter = 0;
399
400 // Running the main loop of the thread
401 while (m_hasToRun) {
402 for (unsigned int id = 0; id < m_vmeshid.size(); id++) {
403 m_vpmutex[id]->lock();
404 unsigned int nbPoints = m_vPointClouds[id]->size();
405 // Checking if the pcl[id] has weights attached to its points
406 unsigned int nbWeights = m_vweights[id].size();
407 useWeights = (nbWeights > 0);
408 if (useWeights) {
409 // Setting points for which the weights are lower than \b s_ignoreThresh to be of color \b s_colorRejectedPoints
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;
415 }
416
417 }
418 }
419
420 // If updatePointCloud fails, it means that the pcl was not previously known by the viewer
421 if (!sp_viewer->updatePointCloud(m_vPointClouds[id], m_vmeshid[id])) {
422 // Add the pcl to the list of pcl known by the viewer + the according legend
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);
425 }
426
427 // If the pcl is not empty and the \b vpPclViewer is asked to save the pcls,
428 // create a new file name and save the pcl
429 if (m_vPointClouds[id]->size() > 0 && m_hasToSavePCDs) {
430 std::string filename = vpIoTools::createFilePath(m_outFolder, m_vmeshid[id] + std::to_string(iter) + ".pcd");
431 pcl::io::savePCDFile(filename, *m_vPointClouds[id]);
432 }
433
434 m_vpmutex[id]->unlock();
435 }
436 sp_viewer->spinOnce(100, true);
437
438 iter++;
439 }
440 // When leaving the thread, resetting the viewer
441 sp_viewer.reset();
442}
443
444void vpPclViewer::threadUpdateSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id)
445{
446 threadUpdateSurface(surface, id, vpColVector());
447}
448
449void vpPclViewer::threadUpdateSurfaceOriginalColor(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id)
450{
452}
453
454void vpPclViewer::threadUpdateSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id, const vpColVector &weights)
455{
456 m_vpmutex[id]->lock();
457 m_vweights[id] = weights; // Saving the weights affected to each point of the pcl
458 unsigned int nbPoints = surface->size();
459 m_vPointClouds[id]->resize(nbPoints); // Resizing internal point cloud to the size of the input surface
460
461 // Iterating on each point of the pcl to change the color of the points
462 // for the default value affected to this pcl
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;
468
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];
472 }
473 m_vpmutex[id]->unlock();
474}
475
476void vpPclViewer::threadUpdateSurfaceOriginalColor(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id, const vpColVector &weights)
477{
478 m_vpmutex[id]->lock();
479 m_vweights[id] = weights; // Saving the weights affected to each point of the pcl
480 unsigned int nbPoints = surface->size();
481 m_vPointClouds[id]->resize(nbPoints); // Resizing internal point cloud to the size of the input surface
482
483 // As we keep the color of the original pcl, a plain copy will do the job
484 pcl::copyPointCloud(*surface, *m_vPointClouds[id]);
485
486 m_vpmutex[id]->unlock();
487}
488
489#elif !defined(VISP_BUILD_SHARED_LIBS)
490// Work around to avoid warning: libvisp_core.a(vpD3DRenderer.cpp.o) has no
491// symbols
492void dummy_vpPCLPointCLoudVisualization() { };
493#endif
494#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
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.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ notInitialized
Used to indicate that a parameter is not initialized.
Definition vpException.h:86
static bool checkDirectory(const std::string &dirname)
static std::string createFilePath(const std::string &parent, const std::string &child)
static void makeDirectory(const std::string &dirname)
static void runThread(vpPclViewer *p_viewer)
Internal method that is called by vpPclViewer::launchThread to launch the drawing thread.
std::string m_title
void display()
Blocking-mode display of the viewer.
static int s_posV
static pcl::visualization::PCLVisualizer::Ptr sp_viewer
void loopThread()
The internal loop of the non-blocking drawing thread.
vpPclViewer(const std::string &title, const int &width=640, const int &height=480, const int &posU=720, const int &posV=560, const std::string &outFolder=std::string(), const double &ignoreThreshold=0.95)
Construct a new vpPclViewer object.
static double s_ignoreThresh
std::thread m_threadDisplay
void setOutFolder(const std::string &outputFolder)
Set the path to the output folder. If different from the empty string, the point clouds will be saved...
std::vector< std::mutex * > m_vpmutex
std::vector< std::string > m_vmeshid
std::vector< vpColVector > m_vweights
pcl::PointXYZRGB pclPointXYZRGB
Definition vpPclViewer.h:70
void refresh(const int &timeout=100, const bool &waitForDrawing=true)
Refresh the display.
void launchThread()
Start the drawing thread that permits to have a non-blocking display.
void setNameWindow(const std::string &nameWindow)
Set the name of the PCL viewer window.
static std::vector< std::vector< double > > s_vhandler
bool m_hasToSavePCDs
static int s_width
struct vpPclViewer::legendParams legendParams
Structure that contains all the required parameters to display a legend on the viewer.
std::string m_outFolder
static int s_posU
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.
std::vector< legendParams > m_vlegends
void threadUpdateSurfaceOriginalColor(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id)
Method to update a point cloud known by the viewer when the drawing thread is running....
static int s_height
pcl::PointCloud< pclPointXYZRGB > pclPointCloudPointXYZRGB
Definition vpPclViewer.h:71
void stopThread()
Stop the drawing thread that permits to have a non-blocking display.
void threadUpdateSurface(const pclPointCloudPointXYZRGB::Ptr &surface, const unsigned int &id)
Method to update a point cloud known by the viewer when the drawing thread is running....
std::vector< pclPointCloudPointXYZRGB::Ptr > m_vPointClouds
void setIgnoreThreshold(const double &thresh)
Set the threshold below which a point must be displayed in black.