Added finite check for points after transform in adjacency octree

PointCloudLibrary · Apr 14, 2014 · c8fa843 · c8fa843
1 parent b582ef4
commit c8fa843
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 8 deletions.
diff --git 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<uint32_t, uint32_t> label_adjacency;
+  super.getSupervoxelAdjacency (label_adjacency);
+
   std::map <uint32_t, pcl::Supervoxel::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<uint32_t, uint32_t> 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
@@ -63,6 +63,8 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
     PointT temp (input_->points[i]);
     if (transform_func_) //Search for point with 
       transform_func_ (temp);
+    if (!pcl::isFinite (temp))
+      continue;
     if (temp.x < minX)
       minX = temp.x;
     if (temp.y < minY)
@@ -106,10 +108,13 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
   {
     PointT temp (point_arg);
     transform_func_ (temp);
-   // calculate integer key for transformed point coordinates
-    key_arg.x = static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
-    key_arg.y = static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
-    key_arg.z = static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_);
+    if (pcl::isFinite (temp))
+    {
+      // calculate integer key for transformed point coordinates
+      key_arg.x = static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
+      key_arg.y = static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
+      key_arg.z = static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_);
+    }
   }
   else 
   {