From ff8b9e542e2c8feacc956de110731e14e857af35 Mon Sep 17 00:00:00 2001 From: Jeremie Papon Date: Tue, 3 Jun 2014 14:58:45 +0200 Subject: [PATCH 1/3] Fix for issue #639 - handle transformed non-finite points correctly --- examples/segmentation/example_supervoxels.cpp | 8 +-- .../impl/octree_pointcloud_adjacency.hpp | 56 ++++++------------- .../pcl/octree/octree_pointcloud_adjacency.h | 5 -- 3 files changed, 20 insertions(+), 49 deletions(-) diff --git a/examples/segmentation/example_supervoxels.cpp b/examples/segmentation/example_supervoxels.cpp index 504e3507099..74d70c084ea 100644 --- a/examples/segmentation/example_supervoxels.cpp +++ b/examples/segmentation/example_supervoxels.cpp @@ -273,6 +273,10 @@ main (int argc, char ** argv) PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters); PointLCloudT::Ptr full_labeled_cloud = super.getLabeledCloud (); + std::cout << "Getting supervoxel adjacency\n"; + std::multimap label_adjacency; + super.getSupervoxelAdjacency (label_adjacency); + std::map ::Ptr > refined_supervoxel_clusters; std::cout << "Refining supervoxels \n"; super.refineSupervoxels (3, refined_supervoxel_clusters); @@ -282,10 +286,6 @@ main (int argc, char ** argv) PointLCloudT::Ptr refined_full_labeled_cloud = super.getLabeledCloud (); PointCloudT::Ptr refined_full_colored_cloud = super.getColoredCloud (); - std::cout << "Getting supervoxel adjacency\n"; - std::multimap label_adjacency; - super.getSupervoxelAdjacency (label_adjacency); - // THESE ONLY MAKE SENSE FOR ORGANIZED CLOUDS pcl::io::savePNGFile (out_path, *full_colored_cloud, "rgb"); pcl::io::savePNGFile (refined_out_path, *refined_full_colored_cloud, "rgb"); diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp index dc3a3fdb28f..15fbe247583 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -62,6 +62,8 @@ pcl::octree::OctreePointCloudAdjacency PointT temp (input_->points[i]); if (transform_func_) //Search for point with transform_func_ (temp); + if (!pcl::isFinite (temp)) //Check to make sure transform didn't make point not finite + continue; if (temp.x < minX) minX = temp.x; if (temp.y < minY) @@ -76,62 +78,29 @@ pcl::octree::OctreePointCloudAdjacency maxZ = temp.z; } this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); - //t1 = timer_.getTime (); - OctreePointCloud::addPointsFromInputCloud (); - - //t2 = timer_.getTime (); - //std::cout << "Add Points:"< > delete_list; - //double t_temp, t_neigh, t_compute, t_getLeaf; - //t_neigh = t_compute = t_getLeaf = 0; + OctreePointCloud::addPointsFromInputCloud (); + LeafContainerT *leaf_container; typename OctreeAdjacencyT::LeafNodeIterator leaf_itr; leaf_vector_.reserve (this->getLeafCount ()); for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr) { - //t_temp = timer_.getTime (); OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey (); leaf_container = &(leaf_itr.getLeafContainer ()); - //t_getLeaf += timer_.getTime () - t_temp; - //t_temp = timer_.getTime (); //Run the leaf's compute function leaf_container->computeData (); - //t_compute += timer_.getTime () - t_temp; - - //t_temp = timer_.getTime (); - // std::cout << "Computing neighbors\n"; + computeNeighbors (leaf_key, leaf_container); - //t_neigh += timer_.getTime () - t_temp; leaf_vector_.push_back (leaf_container); } - //Go through and delete voxels scheduled - for (typename std::list >::iterator delete_itr = delete_list.begin (); delete_itr != delete_list.end (); ++delete_itr) - { - leaf_container = delete_itr->second; - //Remove pointer to it from all neighbors - typename std::set::iterator neighbor_itr = leaf_container->begin (); - typename std::set::iterator neighbor_end = leaf_container->end (); - for ( ; neighbor_itr != neighbor_end; ++neighbor_itr) - { - //Don't delete self neighbor - if (*neighbor_itr != leaf_container) - (*neighbor_itr)->removeNeighbor (leaf_container); - } - this->removeLeaf (delete_itr->first); - } //Make sure our leaf vector is correctly sized assert (leaf_vector_.size () == this->getLeafCount ()); - - // std::cout << "Time spent getting leaves ="< PointT temp (point_arg); transform_func_ (temp); // calculate integer key for transformed point coordinates - key_arg.x = static_cast ((temp.x - this->min_x_) / this->resolution_); - key_arg.y = static_cast ((temp.y - this->min_y_) / this->resolution_); - key_arg.z = static_cast ((temp.z - this->min_z_) / this->resolution_); + if (pcl::isFinite (temp)) //Make sure transformed point is finite - if it is not, it gets default key + { + key_arg.x = static_cast ((temp.x - this->min_x_) / this->resolution_); + key_arg.y = static_cast ((temp.y - this->min_y_) / this->resolution_); + key_arg.z = static_cast ((temp.z - this->min_z_) / this->resolution_); + } + else + { + key = OctreeKey (); + } } else { diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency.h b/octree/include/pcl/octree/octree_pointcloud_adjacency.h index f2f7c525e5f..b0bb33016be 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency.h @@ -49,9 +49,6 @@ #include #include -//DEBUG TODO REMOVE -#include - namespace pcl { @@ -238,8 +235,6 @@ namespace pcl using OctreePointCloudT::max_y_; using OctreePointCloudT::max_z_; - StopWatch timer_; - /// Local leaf pointer vector used to make iterating through leaves fast. LeafVectorT leaf_vector_; From eaa6388e7c2f1d3941927630f27837cbd1632810 Mon Sep 17 00:00:00 2001 From: Jeremie Papon Date: Tue, 3 Jun 2014 15:11:46 +0200 Subject: [PATCH 2/3] Minor formatting --- octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp index 15fbe247583..3109bc45662 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -95,12 +95,9 @@ pcl::octree::OctreePointCloudAdjacency computeNeighbors (leaf_key, leaf_container); leaf_vector_.push_back (leaf_container); - } - //Make sure our leaf vector is correctly sized assert (leaf_vector_.size () == this->getLeafCount ()); - } ////////////////////////////////////////////////////////////////////////////////////////////// From 9a663d0160d4700241d46ec51637daf1d9ee8adb Mon Sep 17 00:00:00 2001 From: Jeremie Papon Date: Tue, 3 Jun 2014 15:41:18 +0200 Subject: [PATCH 3/3] Typo fix --- octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp index 3109bc45662..96288904175 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -117,7 +117,7 @@ pcl::octree::OctreePointCloudAdjacency } else { - key = OctreeKey (); + key_arg = OctreeKey (); } } else