1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 *
35 * \author Raphael Favier
36 * */
37
38 #include <thread>
39
40 #include <pcl/io/auto_io.h>
41 #include <pcl/common/time.h>
42 #include <pcl/visualization/pcl_visualizer.h>
43 #include <pcl/visualization/common/common.h>
44
45 #include <pcl/octree/octree_pointcloud_voxelcentroid.h>
46 #include <pcl/common/centroid.h>
47
48 #include <pcl/filters/filter.h>
49
50 #include <vtkRenderer.h>
51 #include <vtkRenderWindow.h>
52 #include <vtkCubeSource.h>
53 #include <vtkCleanPolyData.h>
54
55 using namespace std::chrono_literals;
56
57 class OctreeViewer
58 {
59 public:
OctreeViewer(std::string & filename,double resolution)60 OctreeViewer (std::string &filename, double resolution) :
61 viz ("Octree visualizator"),
62 cloud (new pcl::PointCloud<pcl::PointXYZ>()),
63 displayCloud (new pcl::PointCloud<pcl::PointXYZ>()),
64 cloudVoxel (new pcl::PointCloud<pcl::PointXYZ>()),
65 octree (resolution),
66 wireframe (true),
67 show_cubes_ (true),
68 show_centroids_ (false),
69 show_original_points_ (false),
70 point_size_ (1.0)
71 {
72
73 //try to load the cloud
74 if (!loadCloud(filename))
75 return;
76
77 //register keyboard callbacks
78 viz.registerKeyboardCallback(&OctreeViewer::keyboardEventOccurred, *this, nullptr);
79
80 //key legends
81 viz.addText ("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t");
82 viz.addText ("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t");
83 viz.addText ("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t");
84 viz.addText ("v -> Toggle octree cubes representation", 10, 125, 0.0, 1.0, 0.0, "key_v_t");
85 viz.addText ("b -> Toggle centroid points representation", 10, 110, 0.0, 1.0, 0.0, "key_b_t");
86 viz.addText ("n -> Toggle original point cloud representation", 10, 95, 0.0, 1.0, 0.0, "key_n_t");
87
88 //set current level to half the maximum one
89 displayedDepth = static_cast<int> (std::floor (octree.getTreeDepth() / 2.0));
90 if (displayedDepth == 0)
91 displayedDepth = 1;
92
93 // assign point cloud to octree
94 octree.setInputCloud (cloud);
95
96 // add points from cloud to octree
97 octree.addPointsFromInputCloud ();
98
99 //show octree at default depth
100 extractPointsAtLevel(displayedDepth);
101
102 //reset camera
103 viz.resetCameraViewpoint("cloud");
104
105 //run main loop
106 run();
107
108 }
109
110 private:
111 //========================================================
112 // PRIVATE ATTRIBUTES
113 //========================================================
114 //visualizer
115 pcl::PointCloud<pcl::PointXYZ>::Ptr xyz;
116
117 pcl::visualization::PCLVisualizer viz;
118 //original cloud
119 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
120 //displayed_cloud
121 pcl::PointCloud<pcl::PointXYZ>::Ptr displayCloud;
122 // cloud which contains the voxel center
123 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVoxel;
124 //octree
125 pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree;
126 //level
127 int displayedDepth;
128 //bool to decide what should be display
129 bool wireframe;
130 bool show_cubes_, show_centroids_, show_original_points_;
131 float point_size_;
132 //========================================================
133
134 /* \brief Callback to interact with the keyboard
135 *
136 */
keyboardEventOccurred(const pcl::visualization::KeyboardEvent & event,void *)137 void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void *)
138 {
139
140 if (event.getKeySym () == "a" && event.keyDown ())
141 {
142 IncrementLevel ();
143 }
144 else if (event.getKeySym () == "z" && event.keyDown ())
145 {
146 DecrementLevel ();
147 }
148 else if (event.getKeySym () == "v" && event.keyDown ())
149 {
150 show_cubes_ = !show_cubes_;
151 update ();
152 }
153 else if (event.getKeySym () == "b" && event.keyDown ())
154 {
155 show_centroids_ = !show_centroids_;
156 update ();
157 }
158 else if (event.getKeySym () == "n" && event.keyDown ())
159 {
160 show_original_points_ = !show_original_points_;
161 update ();
162 }
163 else if (event.getKeySym () == "w" && event.keyDown ())
164 {
165 if (!wireframe)
166 wireframe = true;
167 update ();
168 }
169 else if (event.getKeySym () == "s" && event.keyDown ())
170 {
171 if (wireframe)
172 wireframe = false;
173 update ();
174 }
175 else if ((event.getKeyCode () == '-') && event.keyDown ())
176 {
177 point_size_ = std::max(1.0f, point_size_ * (1 / 2.0f));
178 update ();
179 }
180 else if ((event.getKeyCode () == '+') && event.keyDown ())
181 {
182 point_size_ *= 2.0f;
183 update ();
184 }
185 }
186
187 /* \brief Graphic loop for the viewer
188 *
189 */
run()190 void run()
191 {
192 while (!viz.wasStopped())
193 {
194 //main loop of the visualizer
195 viz.spinOnce(100);
196 std::this_thread::sleep_for(100ms);
197 }
198 }
199
200 /* \brief Helper function that read a pointcloud file (returns false if pbl)
201 * Also initialize the octree
202 *
203 */
loadCloud(std::string & filename)204 bool loadCloud(std::string &filename)
205 {
206 std::cout << "Loading file " << filename.c_str() << std::endl;
207 //read cloud
208 if (pcl::io::load (filename, *cloud))
209 {
210 return false;
211 }
212
213 //remove NaN Points
214 pcl::Indices nanIndexes;
215 pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
216 std::cout << "Loaded " << cloud->size() << " points" << std::endl;
217
218 //create octree structure
219 octree.setInputCloud(cloud);
220 //update bounding box automatically
221 octree.defineBoundingBox();
222 //add points in the tree
223 octree.addPointsFromInputCloud();
224 return true;
225 }
226
227 /* \brief Helper function that draw info for the user on the viewer
228 *
229 */
showLegend()230 void showLegend ()
231 {
232 char dataDisplay[256];
233 sprintf (dataDisplay, "Displaying octree cubes: %s", (show_cubes_) ? ("True") : ("False"));
234 viz.removeShape ("disp_octree_cubes");
235 viz.addText (dataDisplay, 0, 75, 1.0, 0.0, 0.0, "disp_octree_cubes");
236
237 sprintf (dataDisplay, "Displaying centroids voxel: %s", (show_centroids_) ? ("True") : ("False"));
238 viz.removeShape ("disp_centroids_voxel");
239 viz.addText (dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_centroids_voxel");
240
241 sprintf (dataDisplay, "Displaying original point cloud: %s", (show_original_points_) ? ("True") : ("False"));
242 viz.removeShape ("disp_original_points");
243 viz.addText (dataDisplay, 0, 45, 1.0, 0.0, 0.0, "disp_original_points");
244
245 char level[256];
246 sprintf (level, "Displayed depth is %d on %zu", displayedDepth, static_cast<std::size_t>(octree.getTreeDepth()));
247 viz.removeShape ("level_t1");
248 viz.addText (level, 0, 30, 1.0, 0.0, 0.0, "level_t1");
249
250 viz.removeShape ("level_t2");
251 sprintf(level,
252 "Voxel size: %.4fm [%zu voxels]",
253 ::sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
254 static_cast<std::size_t>(cloudVoxel->size()));
255 viz.addText (level, 0, 15, 1.0, 0.0, 0.0, "level_t2");
256 }
257
258 /* \brief Visual update. Create visualizations and add them to the viewer
259 *
260 */
update()261 void update()
262 {
263 //remove existing shapes from visualizer
264 clearView ();
265
266 showLegend ();
267
268 if (show_cubes_)
269 {
270 //show octree as cubes
271 showCubes (::sqrt (octree.getVoxelSquaredSideLen (displayedDepth)));
272 }
273
274 if (show_centroids_)
275 {
276 //show centroid points
277 pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler (cloudVoxel, "x");
278 viz.addPointCloud (cloudVoxel, color_handler, "cloud_centroid");
279 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud_centroid");
280 }
281
282 if (show_original_points_)
283 {
284 //show origin point cloud
285 pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler (cloud, "z");
286 viz.addPointCloud (cloud, color_handler, "cloud");
287 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud");
288 }
289 }
290
291 /* \brief remove dynamic objects from the viewer
292 *
293 */
clearView()294 void clearView()
295 {
296 //remove cubes if any
297 vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
298 while (renderer->GetActors ()->GetNumberOfItems () > 0)
299 renderer->RemoveActor (renderer->GetActors ()->GetLastActor ());
300 //remove point clouds if any
301 viz.removePointCloud ("cloud");
302 viz.removePointCloud ("cloud_centroid");
303 }
304
305
306 /* \brief display octree cubes via vtk-functions
307 *
308 */
showCubes(double voxelSideLen)309 void showCubes(double voxelSideLen)
310 {
311 vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New ();
312
313 // Create every cubes to be displayed
314 double s = voxelSideLen / 2.0;
315 for (const auto &point : cloudVoxel->points)
316 {
317 double x = point.x;
318 double y = point.y;
319 double z = point.z;
320
321 vtkSmartPointer<vtkCubeSource> wk_cubeSource = vtkSmartPointer<vtkCubeSource>::New ();
322
323 wk_cubeSource->SetBounds (x - s, x + s, y - s, y + s, z - s, z + s);
324 wk_cubeSource->Update ();
325
326 appendFilter->AddInputData (wk_cubeSource->GetOutput ());
327 }
328
329 // Remove any duplicate points
330 vtkSmartPointer<vtkCleanPolyData> cleanFilter = vtkSmartPointer<vtkCleanPolyData>::New ();
331
332 cleanFilter->SetInputConnection (appendFilter->GetOutputPort ());
333 cleanFilter->Update ();
334
335 //Create a mapper and actor
336 vtkSmartPointer<vtkPolyDataMapper> multiMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
337
338 multiMapper->SetInputConnection (cleanFilter->GetOutputPort ());
339
340 vtkSmartPointer<vtkActor> multiActor = vtkSmartPointer<vtkActor>::New ();
341
342 multiActor->SetMapper (multiMapper);
343
344 multiActor->GetProperty ()->SetColor (1.0, 1.0, 1.0);
345 multiActor->GetProperty ()->SetAmbient (1.0);
346 multiActor->GetProperty ()->SetLineWidth (1);
347 multiActor->GetProperty ()->EdgeVisibilityOn ();
348 multiActor->GetProperty ()->SetOpacity (1.0);
349
350 if (wireframe)
351 {
352 multiActor->GetProperty ()->SetRepresentationToWireframe ();
353 }
354 else
355 {
356 multiActor->GetProperty ()->SetRepresentationToSurface ();
357 }
358
359 // Add the actor to the scene
360 viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ()->AddActor (multiActor);
361
362 // Render and interact
363 viz.getRenderWindow ()->Render ();
364 }
365
366 /* \brief Extracts all the points of depth = level from the octree
367 *
368 */
extractPointsAtLevel(int depth)369 void extractPointsAtLevel(int depth)
370 {
371 displayCloud->points.clear();
372 cloudVoxel->points.clear();
373
374 pcl::PointXYZ pt_voxel_center;
375 pcl::PointXYZ pt_centroid;
376 std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
377 double start = pcl::getTime ();
378
379 for (pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::FixedDepthIterator tree_it = octree.fixed_depth_begin (depth);
380 tree_it != octree.fixed_depth_end ();
381 ++tree_it)
382 {
383 // Compute the point at the center of the voxel which represents the current OctreeNode
384 Eigen::Vector3f voxel_min, voxel_max;
385 octree.getVoxelBounds (tree_it, voxel_min, voxel_max);
386
387 pt_voxel_center.x = (voxel_min.x () + voxel_max.x ()) / 2.0f;
388 pt_voxel_center.y = (voxel_min.y () + voxel_max.y ()) / 2.0f;
389 pt_voxel_center.z = (voxel_min.z () + voxel_max.z ()) / 2.0f;
390 cloudVoxel->points.push_back (pt_voxel_center);
391
392 // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode
393 if (octree.getTreeDepth () == (unsigned int) depth)
394 {
395 pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode ());
396
397 container->getContainer ().getCentroid (pt_centroid);
398 }
399 // Else, compute the centroid of the LeafNode under the current BranchNode
400 else
401 {
402 // Retrieve every centroid under the current BranchNode
403 pcl::octree::OctreeKey dummy_key;
404 pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids;
405 octree.getVoxelCentroidsRecursive (static_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids);
406
407 // Iterate over the leafs to compute the centroid of all of them
408 pcl::CentroidPoint<pcl::PointXYZ> centroid;
409 for (const auto &voxelCentroid : voxelCentroids)
410 {
411 centroid.add (voxelCentroid);
412 }
413 centroid.get (pt_centroid);
414 }
415
416 displayCloud->points.push_back (pt_centroid);
417 }
418
419 double end = pcl::getTime ();
420 printf("%zu pts, %.4gs. %.4gs./pt. =====\n",
421 static_cast<std::size_t>(displayCloud->size()),
422 end - start,
423 (end - start) / static_cast<double>(displayCloud->size()));
424
425 update();
426 }
427
428 /* \brief Helper function to increase the octree display level by one
429 *
430 */
IncrementLevel()431 bool IncrementLevel()
432 {
433 if (displayedDepth < static_cast<int> (octree.getTreeDepth ()))
434 {
435 displayedDepth++;
436 extractPointsAtLevel(displayedDepth);
437 return true;
438 }
439 return false;
440 }
441
442 /* \brief Helper function to decrease the octree display level by one
443 *
444 */
DecrementLevel()445 bool DecrementLevel()
446 {
447 if (displayedDepth > 0)
448 {
449 displayedDepth--;
450 extractPointsAtLevel(displayedDepth);
451 return true;
452 }
453 return false;
454 }
455
456 };
457
main(int argc,char ** argv)458 int main(int argc, char ** argv)
459 {
460 if (argc != 3)
461 {
462 std::cerr << "ERROR: Syntax is octreeVisu <pcd file> <resolution>" << std::endl;
463 std::cerr << "EXAMPLE: ./octreeVisu bun0.pcd 0.001" << std::endl;
464 return -1;
465 }
466
467 std::string cloud_path(argv[1]);
468 OctreeViewer v(cloud_path, atof(argv[2]));
469 }
470