Imported Upstream version 1.7.2

13 files changed, 299 insertions, 281 deletions

diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp
index 37b5adb0..3191a20e 100644
--- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp
+++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp
@@ -44,7 +44,6 @@
 
 #include <pcl/common/common.h>
 
-using namespace std;
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
@@ -316,13 +315,13 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   min_z_ = min_z_arg;
   max_z_ = max_z_arg;
 
-  min_x_ = min (min_x_, max_x_);
-  min_y_ = min (min_y_, max_y_);
-  min_z_ = min (min_z_, max_z_);
+  min_x_ = std::min (min_x_, max_x_);
+  min_y_ = std::min (min_y_, max_y_);
+  min_z_ = std::min (min_z_, max_z_);
 
-  max_x_ = max (min_x_, max_x_);
-  max_y_ = max (min_y_, max_y_);
-  max_z_ = max (min_z_, max_z_);
+  max_x_ = std::max (min_x_, max_x_);
+  max_y_ = std::max (min_y_, max_y_);
+  max_z_ = std::max (min_z_, max_z_);
 
   // generate bit masks for octree
   getKeyBitSize ();
@@ -351,13 +350,13 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   min_z_ = 0.0f;
   max_z_ = max_z_arg;
 
-  min_x_ = min (min_x_, max_x_);
-  min_y_ = min (min_y_, max_y_);
-  min_z_ = min (min_z_, max_z_);
+  min_x_ = std::min (min_x_, max_x_);
+  min_y_ = std::min (min_y_, max_y_);
+  min_z_ = std::min (min_z_, max_z_);
 
-  max_x_ = max (min_x_, max_x_);
-  max_y_ = max (min_y_, max_y_);
-  max_z_ = max (min_z_, max_z_);
+  max_x_ = std::max (min_x_, max_x_);
+  max_y_ = std::max (min_y_, max_y_);
+  max_z_ = std::max (min_z_, max_z_);
 
   // generate bit masks for octree
   getKeyBitSize ();
@@ -383,13 +382,13 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   min_z_ = 0.0f;
   max_z_ = cubeLen_arg;
 
-  min_x_ = min (min_x_, max_x_);
-  min_y_ = min (min_y_, max_y_);
-  min_z_ = min (min_z_, max_z_);
+  min_x_ = std::min (min_x_, max_x_);
+  min_y_ = std::min (min_y_, max_y_);
+  min_z_ = std::min (min_z_, max_z_);
 
-  max_x_ = max (min_x_, max_x_);
-  max_y_ = max (min_y_, max_y_);
-  max_z_ = max (min_z_, max_z_);
+  max_x_ = std::max (min_x_, max_x_);
+  max_y_ = std::max (min_y_, max_y_);
+  max_z_ = std::max (min_z_, max_z_);
 
   // generate bit masks for octree
   getKeyBitSize ();
@@ -626,11 +625,11 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   max_key_z = static_cast<unsigned int> ((max_z_ - min_z_) / resolution_);
 
   // find maximum amount of keys
-  max_voxels = max (max (max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
+  max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
 
 
   // tree depth == amount of bits of max_voxels
-  this->octree_depth_ = max ((min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
+  this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
                                   static_cast<unsigned int> (0));
 
   octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_-minValue;
@@ -829,7 +828,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
 
-#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >;
-#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >;
+#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
+#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
 
 #endif /* OCTREE_POINTCLOUD_HPP_ */
diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp
index 92963460..96288904 100644
--- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp
+++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp
@@ -54,63 +54,50 @@ template<typename PointT, typename LeafContainerT, typename BranchContainerT> vo
 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud ()
 {
   //double t1,t2;
+  float minX = std::numeric_limits<float>::max (), minY = std::numeric_limits<float>::max (), minZ = std::numeric_limits<float>::max ();
+  float maxX = -std::numeric_limits<float>::max(), maxY = -std::numeric_limits<float>::max(), maxZ = -std::numeric_limits<float>::max();
   
-  //t1 = timer_.getTime ();
-  OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud ();
-
+  for (size_t i = 0; i < input_->size (); ++i)
+  {
+    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)
+      minY = temp.y;
+    if (temp.z < minZ)
+      minZ = temp.z;
+    if (temp.x > maxX)
+      maxX = temp.x;
+    if (temp.y > maxY)
+      maxY = temp.y;
+    if (temp.z > maxZ)
+      maxZ = temp.z;
+  }
+  this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
 
-  //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";
-  
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -122,10 +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 
   {
@@ -147,16 +140,7 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
   const PointT& point = this->input_->points[pointIdx_arg];
   if (!pcl::isFinite (point))
     return;
-  
-  if (transform_func_)
-  { 
-    PointT temp (point);
-    transform_func_ (temp);
-    this->adoptBoundingBoxToPoint (temp);
-  }
-  else  
-    this->adoptBoundingBoxToPoint (point);
-    
+   
   // generate key
   this->genOctreeKeyforPoint (point, key);
   // add point to octree at key
@@ -164,23 +148,34 @@ pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>
   container->addPoint (point);
 }
 
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container)
 { 
+  //Make sure requested key is valid
+  if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y || key_arg.z > this->max_key_.z)
+  {
+    PCL_ERROR ("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for invalid octree key\n");
+    return;
+  }
   
   OctreeKey neighbor_key;
-  
-  for (int dx = -1; dx <= 1; ++dx)
+  int dx_min = (key_arg.x > 0) ? -1 : 0;
+  int dy_min = (key_arg.y > 0) ? -1 : 0;
+  int dz_min = (key_arg.z > 0) ? -1 : 0;
+  int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1;
+  int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1;
+  int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1;
+    
+  for (int dx = dx_min; dx <= dx_max; ++dx)
   {
-    for (int dy = -1; dy <= 1; ++dy)
+    for (int dy = dy_min; dy <= dy_max; ++dy)
     {
-      for (int dz = -1; dz <= 1; ++dz)
+      for (int dz = dz_min; dz <= dz_max; ++dz)
       {
-        neighbor_key.x = key_arg.x + dx;
-        neighbor_key.y = key_arg.y + dy;
-        neighbor_key.z = key_arg.z + dz;
+        neighbor_key.x = static_cast<uint32_t> (key_arg.x + dx);
+        neighbor_key.y = static_cast<uint32_t> (key_arg.y + dy);
+        neighbor_key.z = static_cast<uint32_t> (key_arg.z + dz);
         LeafContainerT *neighbor = this->findLeaf (neighbor_key);
         if (neighbor)
         {
diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp
index 0404e460..51d28f8a 100644
--- a/octree/include/pcl/octree/impl/octree_search.hpp
+++ b/octree/include/pcl/octree/impl/octree_search.hpp
@@ -45,7 +45,6 @@
 #include <pcl/common/common.h>
 #include <assert.h>
 
-using namespace std;
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
@@ -104,7 +103,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::n
   key.x = key.y = key.z = 0;
 
   // initalize smallest point distance in search with high value
-  double smallest_dist = numeric_limits<double>::max ();
+  double smallest_dist = std::numeric_limits<double>::max ();
 
   getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
 
@@ -247,7 +246,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::g
     }
     else
     {
-      search_heap[child_idx].point_distance = numeric_limits<float>::infinity ();
+      search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity ();
     }
   }
 
@@ -276,7 +275,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::g
 
       float squared_dist;
       size_t i;
-      vector<int> decoded_point_vector;
+      std::vector<int> decoded_point_vector;
 
       const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
 
@@ -374,7 +373,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::g
 
         size_t i;
         const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
-        vector<int> decoded_point_vector;
+        std::vector<int> decoded_point_vector;
 
         // decode leaf node into decoded_point_vector
         (*child_leaf)->getPointIndices (decoded_point_vector);
@@ -422,7 +421,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::a
   const OctreeNode* child_node;
 
   // set minimum voxel distance to maximum value
-  min_voxel_center_distance = numeric_limits<double>::max ();
+  min_voxel_center_distance = std::numeric_limits<double>::max ();
 
   min_child_idx = 0xFF;
 
@@ -471,11 +470,11 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::a
     double squared_dist;
     double smallest_squared_dist;
     size_t i;
-    vector<int> decoded_point_vector;
+    std::vector<int> decoded_point_vector;
 
     const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
 
-    smallest_squared_dist = numeric_limits<double>::max ();
+    smallest_squared_dist = std::numeric_limits<double>::max ();
 
     // decode leaf node into decoded_point_vector
     (**child_leaf).getPointIndices (decoded_point_vector);
@@ -557,7 +556,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::b
       {
         // we reached leaf node level
         size_t i;
-        vector<int> decoded_point_vector;
+        std::vector<int> decoded_point_vector;
         bool bInBox;
 
         const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
@@ -602,7 +601,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::g
 
   initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
 
-  if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z))
+  if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
     return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
                                                 voxel_center_list, max_voxel_count);
 
@@ -626,7 +625,7 @@ pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::g
 
   initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
 
-  if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z))
+  if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
     return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
                                                 k_indices, max_voxel_count);
   return (0);
diff --git a/octree/include/pcl/octree/octree2buf_base.h b/octree/include/pcl/octree/octree2buf_base.h
index 6b8dab48..2d6b21ae 100644
--- a/octree/include/pcl/octree/octree2buf_base.h
+++ b/octree/include/pcl/octree/octree2buf_base.h
@@ -805,7 +805,7 @@ namespace pcl
          *  \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
          *  \param key_arg: reference to an octree key
          *  \param binary_tree_in_it_arg iterator of binary input data
-         *  \param leaf_container_vector__it_end_arg end iterator of binary input data
+         *  \param binary_tree_in_it_end_arg
          *  \param leaf_container_vector_it_arg: iterator pointing to leaf containter pointers to be added to a leaf node
          *  \param leaf_container_vector_it_end_arg: iterator pointing to leaf containter pointers pointing to last object in input container.
          *  \param branch_reset_arg: Reset pointer array of current branch
diff --git a/octree/include/pcl/octree/octree_container.h b/octree/include/pcl/octree/octree_container.h
index aa5c20c5..3b06f72b 100644
--- a/octree/include/pcl/octree/octree_container.h
+++ b/octree/include/pcl/octree/octree_container.h
@@ -93,7 +93,7 @@ namespace pcl
        * \return number of points/indices stored in leaf node container.
        */
       virtual size_t
-      getSize ()
+      getSize () const
       {
         return 0u;
       }
diff --git a/octree/include/pcl/octree/octree_iterator.h b/octree/include/pcl/octree/octree_iterator.h
index 48b71f67..0bcb565f 100644
--- a/octree/include/pcl/octree/octree_iterator.h
+++ b/octree/include/pcl/octree/octree_iterator.h
@@ -113,7 +113,6 @@ namespace pcl
         OctreeIteratorBase (const OctreeIteratorBase& src, unsigned int max_depth_arg = 0) :
             octree_ (src.octree_), current_state_(0), max_octree_depth_(max_depth_arg)
         {
-          this->reset ();
         }
 
         /** \brief Copy operator.
@@ -135,7 +134,7 @@ namespace pcl
         }
 
         /** \brief Equal comparison operator
-         * \param[in] OctreeIteratorBase to compare with
+         * \param[in] other OctreeIteratorBase to compare with
          */
         bool operator==(const OctreeIteratorBase& other) const
         {
@@ -145,7 +144,7 @@ namespace pcl
         }
 
         /** \brief Inequal comparison operator
-         * \param[in] OctreeIteratorBase to compare with
+         * \param[in] other OctreeIteratorBase to compare with
          */
         bool operator!=(const OctreeIteratorBase& other) const
         {
diff --git a/octree/include/pcl/octree/octree_nodes.h b/octree/include/pcl/octree/octree_nodes.h
index 13a48c82..d9b26897 100644
--- a/octree/include/pcl/octree/octree_nodes.h
+++ b/octree/include/pcl/octree/octree_nodes.h
@@ -46,6 +46,8 @@
 
 #include <string.h>
 
+#include <Eigen/Core>
+
 #include <pcl/pcl_macros.h>
 
 #include "octree_container.h"
@@ -190,6 +192,10 @@ namespace pcl
 
       protected:
         ContainerT container_;
+        
+      public:
+        //Type ContainerT may have fixed-size Eigen objects inside
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
       };
 
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency.h b/octree/include/pcl/octree/octree_pointcloud_adjacency.h
index 3f61e70f..b0bb3301 100644
--- a/octree/include/pcl/octree/octree_pointcloud_adjacency.h
+++ b/octree/include/pcl/octree/octree_pointcloud_adjacency.h
@@ -40,198 +40,215 @@
 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
 
+#include <pcl/console/print.h>
 #include <pcl/common/geometry.h>
 #include <pcl/octree/boost.h>
-#include <pcl/octree/octree_base.h>
-#include <pcl/octree/octree_iterator.h>
-#include <pcl/octree/octree_pointcloud.h>
+#include <pcl/octree/octree_impl.h>
 #include <pcl/octree/octree_pointcloud_adjacency_container.h>
 
 #include <set>
 #include <list>
 
-//DEBUG TODO REMOVE
-#include <pcl/common/time.h>
-
-
-
 namespace pcl
-{ 
-  
+{
+
   namespace octree
   {
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    /** \brief @b Octree pointcloud voxel class used for adjacency calculation 
-     *  \note This pointcloud octree class generate an octree from a point cloud (zero-copy). 
-     *  \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
-     *  \note This class maintains adjacency information for its voxels 
-     *  \note The OctreePointCloudAdjacencyContainer can be used to store data in leaf nodes
-     *  \note An optional transform function can be provided which changes how the voxel grid is computed - this can be used to, for example, make voxel bins larger as they increase in distance from the origin (camera)
-     *  \note See SupervoxelClustering for an example of how to provide a transform function 
-     *  \note If used in academic work, please cite:
-     * 
-     * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
-     *   Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
-     *   In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 
-     *  \ingroup octree
-     *  \author Jeremie Papon (jpapon@gmail.com)
-     */
+    /** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels.
+      *
+      * This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is
+      * initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
+      *
+      * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes.
+      *
+      * An optional transform function can be provided which changes how the voxel grid is computed - this can be used to,
+      * for example, make voxel bins larger as they increase in distance from the origin (camera).
+      * \note See SupervoxelClustering for an example of how to provide a transform function.
+      *
+      * If used in academic work, please cite:
+      *
+      * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
+      *   Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
+      *   In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
+      *
+      * \ingroup octree
+      * \author Jeremie Papon (jpapon@gmail.com) */
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    template< typename PointT, 
-    typename LeafContainerT = OctreePointCloudAdjacencyContainer <PointT>,    
-    typename BranchContainerT = OctreeContainerEmpty >
-    class OctreePointCloudAdjacency : public OctreePointCloud< PointT, LeafContainerT, BranchContainerT>
-    
+    template <typename PointT,
+              typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
+              typename BranchContainerT = OctreeContainerEmpty>
+    class OctreePointCloudAdjacency : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
     {
-      
-    public:
-      typedef OctreeBase<LeafContainerT, BranchContainerT> OctreeBaseT;
-      
-      typedef OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT;
-      typedef boost::shared_ptr<OctreeAdjacencyT> Ptr;
-      typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr;
-      
-      typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT,OctreeBaseT > OctreePointCloudT;
-      typedef typename OctreePointCloudT::LeafNode LeafNode;
-      typedef typename OctreePointCloudT::BranchNode BranchNode;
-      
-      typedef pcl::PointCloud<PointT> CloudT;
-      
-      //So we can access input
-      using OctreePointCloudT::input_;
-      using OctreePointCloudT::resolution_;
-      
-      // iterators are friends
-      friend class OctreeIteratorBase<OctreeAdjacencyT> ;
-      friend class OctreeDepthFirstIterator<OctreeAdjacencyT> ;
-      friend class OctreeBreadthFirstIterator<OctreeAdjacencyT> ;
-      friend class OctreeLeafNodeIterator<OctreeAdjacencyT> ;
-      
-      // Octree default iterators
-      typedef OctreeDepthFirstIterator<OctreeAdjacencyT> Iterator;
-      typedef const OctreeDepthFirstIterator<OctreeAdjacencyT> ConstIterator;
-      Iterator depth_begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);};
-      const Iterator depth_end() {return Iterator();};
-      
-      // Octree leaf node iterators
-      typedef OctreeLeafNodeIterator<OctreeAdjacencyT> LeafNodeIterator;
-      typedef const OctreeLeafNodeIterator< OctreeAdjacencyT> ConstLeafNodeIterator;
-      LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);};
-      const LeafNodeIterator leaf_end() {return LeafNodeIterator();};
-           
-      typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList;
-      typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID;
-      typedef typename VoxelAdjacencyList::edge_descriptor EdgeID;
-      
-      //Leaf vector - pointers to all leaves
-      typedef std::vector <LeafContainerT*> LeafVectorT;
-      //Fast leaf iterators that don't require traversing tree
-      typedef typename LeafVectorT::iterator iterator;
-      typedef typename LeafVectorT::const_iterator const_iterator;
-      inline iterator begin () { return (leaf_vector_.begin ()); }
-      inline iterator end ()   { return (leaf_vector_.end ()); }
-      //size of neighbors
-      inline size_t size () const { return leaf_vector_.size (); }
-      
-      
-      /** \brief Constructor.
-       *  \param[in] resolution_arg  octree resolution at lowest octree level (voxel size)
-      **/
-      OctreePointCloudAdjacency (const double resolution_arg);
-      
-      
-      /** \brief Empty class destructor. */
-      virtual ~OctreePointCloudAdjacency ()
-      {
-      }
-      
-      /** \brief Adds points from cloud to the octree  
-       *  \note This overrides the addPointsFromInputCloud from the OctreePointCloud class
-       */
-      void 
-      addPointsFromInputCloud ();
-      
-      /** \brief Gets the leaf container for a given point 
-       *  \param[in] point_arg Point to search for
-       *  \returns Pointer to the leaf container - null if no leaf container found
-       */
-      LeafContainerT *
-      getLeafContainerAtPoint (const PointT& point_arg) const;
-
-      /** \brief Computes an adjacency graph of voxel relations 
-       * \note WARNING: This slows down rapidly as cloud size increases due number of edges 
-       * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships - vertices are pointT, edges represent touching, and edge lengths are the distance between the points
-       */
-      void
-      computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
-      
-      /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud
-       *  This is useful for changing how adjacency is calculated - such as relaxing
-       *  the adjacency criterion for points further from the camera
-       *  \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one parameter (a point) which it modifies in place
-       */
-      void 
-      setTransformFunction (boost::function<void (PointT &p)> transform_func)
-      {
-        transform_func_ = transform_func;
-      }
-        
-      /** \brief Tests whether input point is occluded from specified camera point by other voxels
-          \param[in] point_arg Point to test for
-          \param[in] camera_pos Position of camera, defaults to origin
-          \returns True if path to camera is blocked by a voxel, false otherwise
-      */
-      bool
-      testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0,0,0));
-        
-    protected:
-      
-      /** \brief Add point at index from input pointcloud dataset to octree
-       * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added
-       * \note This virtual implementation allows the use of a transform function to compute keys
-       */
-       virtual void                         
-       addPointIdx (const int pointIdx_arg);
-       
-      /** \brief Fills in the neighbors fields for new voxels 
-       *  \param[in] key_arg Key of the voxel to check neighbors for
-       *  \param[in] leaf_container Pointer to container of the leaf to check neighbors for
-       */
-      void
-      computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
-
-      /** \brief Generates octree key for specified point (uses transform if provided)
-       *  \param[in] point_arg Point to generate key for
-       *  \param[out] key_arg Resulting octree key
-       */
-      void
-      genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const;
-      
-          private:
-
-      StopWatch timer_;
-
-      //Local leaf pointer vector used to make iterating through leaves fast
-      LeafVectorT leaf_vector_;
-
-      boost::function<void (PointT &p)> transform_func_;
-    };
-    
-  }  
-  
-}
-
-
-
-
 
+      public:
+
+        typedef OctreeBase<LeafContainerT, BranchContainerT> OctreeBaseT;
+
+        typedef OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT> OctreeAdjacencyT;
+        typedef boost::shared_ptr<OctreeAdjacencyT> Ptr;
+        typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr;
+
+        typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeBaseT> OctreePointCloudT;
+        typedef typename OctreePointCloudT::LeafNode LeafNode;
+        typedef typename OctreePointCloudT::BranchNode BranchNode;
+
+        typedef pcl::PointCloud<PointT> PointCloud;
+        typedef boost::shared_ptr<PointCloud> PointCloudPtr;
+        typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
+
+        // Iterators are friends
+        friend class OctreeIteratorBase<OctreeAdjacencyT>;
+        friend class OctreeDepthFirstIterator<OctreeAdjacencyT>;
+        friend class OctreeBreadthFirstIterator<OctreeAdjacencyT>;
+        friend class OctreeLeafNodeIterator<OctreeAdjacencyT>;
+
+        // Octree default iterators
+        typedef OctreeDepthFirstIterator<OctreeAdjacencyT> Iterator;
+        typedef const OctreeDepthFirstIterator<OctreeAdjacencyT> ConstIterator;
+
+        Iterator depth_begin (unsigned int max_depth_arg = 0) { return Iterator (this, max_depth_arg); }
+        const Iterator depth_end () { return Iterator (); }
+
+        // Octree leaf node iterators
+        typedef OctreeLeafNodeIterator<OctreeAdjacencyT> LeafNodeIterator;
+        typedef const OctreeLeafNodeIterator<OctreeAdjacencyT> ConstLeafNodeIterator;
+
+        LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0) { return LeafNodeIterator (this, max_depth_arg); }
+        const LeafNodeIterator leaf_end () { return LeafNodeIterator (); }
+
+        // BGL graph
+        typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList;
+        typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID;
+        typedef typename VoxelAdjacencyList::edge_descriptor EdgeID;
+
+        // Leaf vector - pointers to all leaves
+        typedef std::vector<LeafContainerT*> LeafVectorT;
+
+        // Fast leaf iterators that don't require traversing tree
+        typedef typename LeafVectorT::iterator iterator;
+        typedef typename LeafVectorT::const_iterator const_iterator;
+
+        inline iterator begin () { return (leaf_vector_.begin ()); }
+        inline iterator end ()   { return (leaf_vector_.end ()); }
+
+        // Size of neighbors
+        inline size_t size () const { return leaf_vector_.size (); }
+
+        /** \brief Constructor.
+          *
+          * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */
+        OctreePointCloudAdjacency (const double resolution_arg);
+
+        /** \brief Empty class destructor. */
+        virtual ~OctreePointCloudAdjacency ()
+        {
+        }
+
+        /** \brief Adds points from cloud to the octree.
+          *
+          * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */
+        void
+        addPointsFromInputCloud ();
+
+        /** \brief Gets the leaf container for a given point.
+          *
+          * \param[in] point_arg Point to search for
+          *
+          * \returns Pointer to the leaf container - null if no leaf container found. */
+        LeafContainerT*
+        getLeafContainerAtPoint (const PointT& point_arg) const;
+
+        /** \brief Computes an adjacency graph of voxel relations.
+          *
+          * \warning This slows down rapidly as cloud size increases due to the number of edges.
+          *
+          * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships.
+          * Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */
+        void
+        computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
+
+        /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud.
+          *
+          * This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for
+          * points further from the camera.
+          *
+          * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one
+          * parameter (a point) which it modifies in place. */
+        void
+        setTransformFunction (boost::function<void (PointT &p)> transform_func)
+        {
+          transform_func_ = transform_func;
+        }
+
+        /** \brief Tests whether input point is occluded from specified camera point by other voxels.
+          *
+          * \param[in] point_arg Point to test for
+          * \param[in] camera_pos Position of camera, defaults to origin
+          *
+          * \returns True if path to camera is blocked by a voxel, false otherwise. */
+        bool
+        testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0));
+
+      protected:
+
+        /** \brief Add point at index from input pointcloud dataset to octree.
+          *
+          * \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added
+          *
+          * \note This virtual implementation allows the use of a transform function to compute keys. */
+         virtual void
+         addPointIdx (const int point_idx_arg);
+
+        /** \brief Fills in the neighbors fields for new voxels.
+          *
+          * \param[in] key_arg Key of the voxel to check neighbors for
+          * \param[in] leaf_container Pointer to container of the leaf to check neighbors for */
+        void
+        computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
+
+        /** \brief Generates octree key for specified point (uses transform if provided).
+          *
+          * \param[in] point_arg Point to generate key for
+          * \param[out] key_arg Resulting octree key */
+        void
+        genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const;
+
+      private:
+
+        /** \brief Add point at given index from input point cloud to octree.
+          *
+          * Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */
+        using OctreePointCloudT::addPointFromCloud;
+
+        /** \brief Add point simultaneously to octree and input point cloud.
+          *
+          * This functionality is not enabled for adjacency octree. */
+        using OctreePointCloudT::addPointToCloud;
+
+        using OctreePointCloudT::input_;
+        using OctreePointCloudT::resolution_;
+        using OctreePointCloudT::min_x_;
+        using OctreePointCloudT::min_y_;
+        using OctreePointCloudT::min_z_;
+        using OctreePointCloudT::max_x_;
+        using OctreePointCloudT::max_y_;
+        using OctreePointCloudT::max_z_;
+
+        /// Local leaf pointer vector used to make iterating through leaves fast.
+        LeafVectorT leaf_vector_;
+
+        boost::function<void (PointT &p)> transform_func_;
 
+    };
 
+  }
 
+}
 
 //#ifdef PCL_NO_PRECOMPILE
 #include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
 //#endif
 
-#endif //PCL_OCTREE_POINTCLOUD_SUPER_VOXEL_H_
+#endif // PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
 
diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h
index d31ac110..326c0f54 100644
--- a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h
+++ b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h
@@ -92,10 +92,10 @@ namespace pcl
       /** \brief Add new point to container- this just counts points
        * \note To actually store data in the leaves, need to specialize this
        * for your point and data type as in supervoxel_clustering.hpp
-       * \param[in] new_point the new point to add  
        */
+       // param[in] new_point the new point to add  
       void 
-      addPoint (const PointInT& new_point)
+      addPoint (const PointInT& /*new_point*/)
       {
         using namespace pcl::common;
         ++num_points_;
@@ -176,7 +176,7 @@ namespace pcl
        * \return number of points added to leaf node container.
        */
       virtual size_t
-      getSize ()
+      getSize () const
       {
         return num_points_;
       }
@@ -190,4 +190,4 @@ namespace pcl
   }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/octree/include/pcl/octree/octree_pointcloud_changedetector.h b/octree/include/pcl/octree/octree_pointcloud_changedetector.h
index d64cd9ef..cdacc074 100644
--- a/octree/include/pcl/octree/octree_pointcloud_changedetector.h
+++ b/octree/include/pcl/octree/octree_pointcloud_changedetector.h
@@ -98,7 +98,7 @@ namespace pcl
 
           for (it=leaf_containers.begin(); it!=it_end; ++it)
           {
-            if ((*it)->getSize()>=minPointsPerLeaf_arg)
+            if (static_cast<int> ((*it)->getSize ()) >= minPointsPerLeaf_arg)
               (*it)->getPointIndices(indicesVector_arg);
           }
 
diff --git a/octree/include/pcl/octree/octree_pointcloud_occupancy.h b/octree/include/pcl/octree/octree_pointcloud_occupancy.h
index 93e5c67f..53413c45 100644
--- a/octree/include/pcl/octree/octree_pointcloud_occupancy.h
+++ b/octree/include/pcl/octree/octree_pointcloud_occupancy.h
@@ -102,7 +102,7 @@ namespace pcl
             genOctreeKeyforPoint (point_arg, key);
 
             // add point to octree at key
-            this->addData (key, 0);
+            this->createLeaf (key);
         }
 
         /** \brief Set occupied voxels at all points from point cloud.
diff --git a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
index 8f3dbae0..10cddadc 100644
--- a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
+++ b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
@@ -77,8 +77,8 @@ namespace pcl
         }
 
         /** \brief Equal comparison operator - set to false
-         * \param[in] OctreePointCloudVoxelCentroidContainer to compare with
          */
+         // param[in] OctreePointCloudVoxelCentroidContainer to compare with
         virtual bool operator==(const OctreeContainerBase&) const
         {
           return ( false );
@@ -168,8 +168,7 @@ namespace pcl
         }
 
         /** \brief Add DataT object to leaf node at octree key.
-          * \param[in] key_arg octree key addressing a leaf node.
-          * \param[in] data_arg DataT object to be added.
+          * \param pointIdx_arg
           */
         virtual void 
         addPointIdx (const int pointIdx_arg)
diff --git a/octree/include/pcl/octree/octree_search.h b/octree/include/pcl/octree/octree_search.h
index 5d1ce079..5c039bf3 100644
--- a/octree/include/pcl/octree/octree_search.h
+++ b/octree/include/pcl/octree/octree_search.h
@@ -600,6 +600,10 @@ namespace pcl
   }
 }
 
+#ifdef PCL_NO_PRECOMPILE
+#include <pcl/octree/impl/octree_search.hpp>
+#else
 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
+#endif    // PCL_NO_PRECOMPILE
 
 #endif    // PCL_OCTREE_SEARCH_H_