Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[OCTREE] Compute accurately the centroids of octree in 'pcl_octree_viewer' #1981

Merged
merged 2 commits into from
Sep 21, 2017
Merged

[OCTREE] Compute accurately the centroids of octree in 'pcl_octree_viewer' #1981

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aede78e
[OCTREE] Compute accurately the centroid of octree in 'octree_viewer'.
Aug 31, 2017
6dadd0b
[OCTREE] Save the point size setting between different depth.
Sep 7, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
230 changes: 150 additions & 80 deletions tools/octree_viewer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <pcl/visualization/common/common.h>

#include <pcl/octree/octree_pointcloud_voxelcentroid.h>
#include <pcl/common/centroid.h>

#include <pcl/filters/filter.h>
#include "boost.h"
Expand All @@ -60,9 +61,16 @@ class OctreeViewer
{
public:
OctreeViewer (std::string &filename, double resolution) :
viz ("Octree visualizator"), cloud (new pcl::PointCloud<pcl::PointXYZ>()),
displayCloud (new pcl::PointCloud<pcl::PointXYZ>()), octree (resolution), displayCubes(false),
showPointsWithCubes (false), wireframe (true)
viz ("Octree visualizator"),
cloud (new pcl::PointCloud<pcl::PointXYZ>()),
displayCloud (new pcl::PointCloud<pcl::PointXYZ>()),
cloudVoxel (new pcl::PointCloud<pcl::PointXYZ>()),
octree (resolution),
wireframe (true),
show_cubes_ (true),
show_centroids_ (false),
show_original_points_ (false),
point_size_ (1.0)
{

//try to load the cloud
Expand All @@ -73,18 +81,24 @@ class OctreeViewer
viz.registerKeyboardCallback(&OctreeViewer::keyboardEventOccurred, *this, 0);

//key legends
viz.addText("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t");
viz.addText("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t");
viz.addText("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t");
viz.addText("d -> Toggle Point/Cube representation", 10, 125, 0.0, 1.0, 0.0, "key_d_t");
viz.addText("x -> Show/Hide original cloud", 10, 110, 0.0, 1.0, 0.0, "key_x_t");
viz.addText("s/w -> Surface/Wireframe representation", 10, 95, 0.0, 1.0, 0.0, "key_sw_t");
viz.addText ("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t");
viz.addText ("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t");
viz.addText ("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t");
viz.addText ("v -> Toggle octree cubes representation", 10, 125, 0.0, 1.0, 0.0, "key_v_t");
viz.addText ("b -> Toggle centroid points representation", 10, 110, 0.0, 1.0, 0.0, "key_b_t");
viz.addText ("n -> Toggle original point cloud representation", 10, 95, 0.0, 1.0, 0.0, "key_n_t");

//set current level to half the maximum one
displayedDepth = static_cast<int> (floor (octree.getTreeDepth() / 2.0));
if (displayedDepth == 0)
displayedDepth = 1;

// assign point cloud to octree
octree.setInputCloud (cloud);

// add points from cloud to octree
octree.addPointsFromInputCloud ();

//show octree at default depth
extractPointsAtLevel(displayedDepth);

Expand All @@ -108,49 +122,68 @@ class OctreeViewer
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//displayed_cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr displayCloud;
// cloud which contains the voxel center
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVoxel;
//octree
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree;
//level
int displayedDepth;
//bool to decide if we display points or cubes
bool displayCubes, showPointsWithCubes, wireframe;
//bool to decide what should be display
bool wireframe;
bool show_cubes_, show_centroids_, show_original_points_;
float point_size_;
//========================================================

/* \brief Callback to interact with the keyboard
*
*/
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *)
void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void *)
{

if (event.getKeySym() == "a" && event.keyDown())
if (event.getKeySym () == "a" && event.keyDown ())
{
IncrementLevel ();
}
else if (event.getKeySym () == "z" && event.keyDown ())
{
IncrementLevel();
DecrementLevel ();
}
else if (event.getKeySym() == "z" && event.keyDown())
else if (event.getKeySym () == "v" && event.keyDown ())
{
DecrementLevel();
show_cubes_ = !show_cubes_;
update ();
}
else if (event.getKeySym() == "d" && event.keyDown())
else if (event.getKeySym () == "b" && event.keyDown ())
{
displayCubes = !displayCubes;
update();
show_centroids_ = !show_centroids_;
update ();
}
else if (event.getKeySym() == "x" && event.keyDown())
else if (event.getKeySym () == "n" && event.keyDown ())
{
showPointsWithCubes = !showPointsWithCubes;
update();
show_original_points_ = !show_original_points_;
update ();
}
else if (event.getKeySym() == "w" && event.keyDown())
else if (event.getKeySym () == "w" && event.keyDown ())
{
if(!wireframe)
wireframe=true;
update();
if (!wireframe)
wireframe = true;
update ();
}
else if (event.getKeySym() == "s" && event.keyDown())
else if (event.getKeySym () == "s" && event.keyDown ())
{
if(wireframe)
wireframe=false;
update();
if (wireframe)
wireframe = false;
update ();
}
else if ((event.getKeyCode () == '-') && event.keyDown ())
{
point_size_ = std::max(1.0f, point_size_ * (1 / 2.0f));
update ();
}
else if ((event.getKeyCode () == '+') && event.keyDown ())
{
point_size_ *= 2.0f;
update ();
}
}

Expand Down Expand Up @@ -197,26 +230,30 @@ class OctreeViewer
/* \brief Helper function that draw info for the user on the viewer
*
*/
void showLegend(bool showCubes)
void showLegend ()
{
char dataDisplay[256];
sprintf(dataDisplay, "Displaying data as %s", (showCubes) ? ("CUBES") : ("POINTS"));
viz.removeShape("disp_t");
viz.addText(dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_t");
sprintf (dataDisplay, "Displaying octree cubes: %s", (show_cubes_) ? ("True") : ("False"));
viz.removeShape ("disp_octree_cubes");
viz.addText (dataDisplay, 0, 75, 1.0, 0.0, 0.0, "disp_octree_cubes");

sprintf (dataDisplay, "Displaying centroids voxel: %s", (show_centroids_) ? ("True") : ("False"));
viz.removeShape ("disp_centroids_voxel");
viz.addText (dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_centroids_voxel");

sprintf (dataDisplay, "Displaying original point cloud: %s", (show_original_points_) ? ("True") : ("False"));
viz.removeShape ("disp_original_points");
viz.addText (dataDisplay, 0, 45, 1.0, 0.0, 0.0, "disp_original_points");

char level[256];
sprintf(level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
viz.removeShape("level_t1");
viz.addText(level, 0, 45, 1.0, 0.0, 0.0, "level_t1");

viz.removeShape("level_t2");
sprintf(level, "Voxel size: %.4fm [%lu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
displayCloud->points.size());
viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2");

viz.removeShape("org_t");
if (showPointsWithCubes)
viz.addText("Displaying original cloud", 0, 15, 1.0, 0.0, 0.0, "org_t");
sprintf (level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
viz.removeShape ("level_t1");
viz.addText (level, 0, 30, 1.0, 0.0, 0.0, "level_t1");

viz.removeShape ("level_t2");
sprintf (level, "Voxel size: %.4fm [%lu voxels]", std::sqrt (octree.getVoxelSquaredSideLen (displayedDepth)),
cloudVoxel->points.size ());
viz.addText (level, 0, 15, 1.0, 0.0, 0.0, "level_t2");
}

/* \brief Visual update. Create visualizations and add them to the viewer
Expand All @@ -225,29 +262,30 @@ class OctreeViewer
void update()
{
//remove existing shapes from visualizer
clearView();
clearView ();

//prevent the display of too many cubes
bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;
showLegend ();

showLegend(displayCubeLegend);

if (displayCubeLegend)
if (show_cubes_)
{
//show octree as cubes
showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
if (showPointsWithCubes)
{
//add original cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
viz.addPointCloud(cloud, color_handler, "cloud");
}
showCubes (std::sqrt (octree.getVoxelSquaredSideLen (displayedDepth)));
}
else

if (show_centroids_)
{
//add current cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud,"z");
viz.addPointCloud(displayCloud, color_handler, "cloud");
//show centroid points
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler (cloudVoxel, "x");
viz.addPointCloud (cloudVoxel, color_handler, "cloud_centroid");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud_centroid");
}

if (show_original_points_)
{
//show origin point cloud
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler (cloud, "z");
viz.addPointCloud (cloud, color_handler, "cloud");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud");
}
}

Expand All @@ -257,11 +295,12 @@ class OctreeViewer
void clearView()
{
//remove cubes if any
vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer();
while (renderer->GetActors()->GetNumberOfItems() > 0)
renderer->RemoveActor(renderer->GetActors()->GetLastActor());
vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
while (renderer->GetActors ()->GetNumberOfItems () > 0)
renderer->RemoveActor (renderer->GetActors ()->GetLastActor ());
//remove point clouds if any
viz.removePointCloud("cloud");
viz.removePointCloud ("cloud");
viz.removePointCloud ("cloud_centroid");
}


Expand All @@ -274,11 +313,11 @@ class OctreeViewer

// Create every cubes to be displayed
double s = voxelSideLen / 2.0;
for (size_t i = 0; i < displayCloud->points.size (); i++)
for (size_t i = 0; i < cloudVoxel->points.size (); i++)
{
double x = displayCloud->points[i].x;
double y = displayCloud->points[i].y;
double z = displayCloud->points[i].z;
double x = cloudVoxel->points[i].x;
double y = cloudVoxel->points[i].y;
double z = cloudVoxel->points[i].z;

vtkSmartPointer<vtkCubeSource> wk_cubeSource = vtkSmartPointer<vtkCubeSource>::New ();

Expand Down Expand Up @@ -309,12 +348,13 @@ class OctreeViewer

multiActor->GetProperty ()->SetColor (1.0, 1.0, 1.0);
multiActor->GetProperty ()->SetAmbient (1.0);
multiActor->GetProperty ()->SetLineWidth (2);
multiActor->GetProperty ()->SetLineWidth (1);
multiActor->GetProperty ()->EdgeVisibilityOn ();
multiActor->GetProperty ()->SetOpacity (1.0);

if (wireframe)
{
multiActor->GetProperty ()->SetRepresentationToWireframe ();
multiActor->GetProperty ()->EdgeVisibilityOn ();
multiActor->GetProperty ()->SetOpacity (1.0);
}
else
{
Expand All @@ -334,26 +374,56 @@ class OctreeViewer
void extractPointsAtLevel(int depth)
{
displayCloud->points.clear();
cloudVoxel->points.clear();

pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it;
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it_end = octree.end();

pcl::PointXYZ pt;
pcl::PointXYZ pt_voxel_center;
pcl::PointXYZ pt_centroid;
std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
double start = pcl::getTime ();

for (tree_it = octree.begin(depth); tree_it!=tree_it_end; ++tree_it)
{
// If the iterator is not at the right depth, continue
if (tree_it.getCurrentOctreeDepth () != depth)
continue;

// Compute the point at the center of the voxel which represents the current OctreeNode
Eigen::Vector3f voxel_min, voxel_max;
octree.getVoxelBounds(tree_it, voxel_min, voxel_max);
octree.getVoxelBounds (tree_it, voxel_min, voxel_max);

pt_voxel_center.x = (voxel_min.x () + voxel_max.x ()) / 2.0f;
pt_voxel_center.y = (voxel_min.y () + voxel_max.y ()) / 2.0f;
pt_voxel_center.z = (voxel_min.z () + voxel_max.z ()) / 2.0f;
cloudVoxel->points.push_back (pt_voxel_center);

// If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode
if (octree.getTreeDepth () == depth)
{
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode ());

container->getContainer ().getCentroid (pt_centroid);
}
// Else, compute the centroid of the LeafNode under the current BranchNode
else
{
// Retrieve every centroid under the current BranchNode
pcl::octree::OctreeKey dummy_key;
pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids;
octree.getVoxelCentroidsRecursive (static_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::BranchNode*> (*tree_it), dummy_key, voxelCentroids);

// Iterate over the leafs to compute the centroid of all of them
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We have a dedicated utility class for this: CentroidPoint.

// Add this include
#include <pcl/common/centroid.h>

// Utility class, instantiate before the loop
CentroidPoint<pcl::PointXYZ> centroid;

// Inside the loop add points
centroid.add (voxelCentroids[j]);

// Fetch centroid after the loop
centroid.get (pt_centroid);

Note that the line 410 is not necessary.

pcl::CentroidPoint<pcl::PointXYZ> centroid;
for (int j = 0; j < voxelCentroids.size (); ++j)
{
centroid.add (voxelCentroids[j]);
}
centroid.get (pt_centroid);
}

pt.x = (voxel_min.x() + voxel_max.x()) / 2.0f;
pt.y = (voxel_min.y() + voxel_max.y()) / 2.0f;
pt.z = (voxel_min.z() + voxel_max.z()) / 2.0f;
displayCloud->points.push_back(pt);
displayCloud->points.push_back (pt_centroid);
}

double end = pcl::getTime ();
Expand Down