Adjacency finite fix

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<PointT>::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");

octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp

@@ -62,6 +62,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)) //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,26 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
       maxZ = temp.z;
   }
   this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
-  //t1 = timer_.getTime ();
-  OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud ();
-
 
-  //t2 = timer_.getTime ();
-  //std::cout << "Add Points:"<<t2-t1<<" ms  Num leaves ="<<this->getLeafCount ()<<"\n";
-
-  std::list <std::pair<OctreeKey,LeafContainerT*> > delete_list;
-  //double t_temp, t_neigh, t_compute, t_getLeaf;
-  //t_neigh = t_compute = t_getLeaf = 0;
+  OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::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<std::pair<OctreeKey,LeafContainerT*> >::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<LeafContainerT*>::iterator neighbor_itr = leaf_container->begin ();
-    typename std::set<LeafContainerT*>::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 ="<<t_getLeaf<<"\n";
- // std::cout << "Time spent computing data in leaves="<<t_compute<<"\n";
- // std::cout << "Time spent computing neighbors="<<t_neigh<<"\n";
-
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -143,9 +109,16 @@ 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)) //Make sure transformed point is finite - if it is not, it gets default key
+    {
+      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
+    {
+      key_arg = OctreeKey ();
+    }
   }
   else 
   {

octree/include/pcl/octree/octree_pointcloud_adjacency.h

@@ -49,9 +49,6 @@
 #include <set>
 #include <list>
 
-//DEBUG TODO REMOVE
-#include <pcl/common/time.h>
-
 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_;