diff --git a/README.md b/README.md
index 4c2c0c72..9901ee87 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,7 @@
 octomap_mapping
 ===============
+  
+Forked by Yonder Dynamics to implement latest features of octomap lib that allow for a 10x speed up.
 
 ROS stack for mapping with OctoMap, contains the octomap_server package.
 
diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp
index c8e11b54..612d8806 100644
--- a/octomap_server/src/OctomapServer.cpp
+++ b/octomap_server/src/OctomapServer.cpp
@@ -361,7 +361,15 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl
   {
     ROS_ERROR_STREAM("Could not generate Key for origin "<<sensorOrigin);
   }
+  // Generate octomap pointcloud
+  Pointcloud pc;
+  for (PCLPointCloud::const_iterator it = nonground.begin(); it != nonground.end(); ++it){
+    pc.push_back(it->x, it->y, it->z);
+  }
+  m_octree->insertPointCloud (pc, sensorOrigin, m_maxRange, true, true);
+  m_octree->updateInnerOccupancy();
 
+  /*
 #ifdef COLOR_OCTOMAP_SERVER
   unsigned char* colors = new unsigned char[3];
 #endif
@@ -471,9 +479,6 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl
   ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<<maxPt);
   ROS_DEBUG_STREAM("Bounding box keys (after): " << m_updateBBXMin[0] << " " <<m_updateBBXMin[1] << " " << m_updateBBXMin[2] << " / " <<m_updateBBXMax[0] << " "<<m_updateBBXMax[1] << " "<< m_updateBBXMax[2]);
 
-  if (m_compressMap)
-    m_octree->prune();
-
 #ifdef COLOR_OCTOMAP_SERVER
   if (colors)
   {
@@ -481,6 +486,9 @@ void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCl
     colors = NULL;
   }
 #endif
+  */
+  if (m_compressMap)
+    m_octree->prune();
 }
 
 
diff --git a/octomap_server/src/octomap_server_static.cpp b/octomap_server/src/octomap_server_static.cpp
index 10bec478..b7962a23 100644
--- a/octomap_server/src/octomap_server_static.cpp
+++ b/octomap_server/src/octomap_server_static.cpp
@@ -143,6 +143,7 @@ int main(int argc, char** argv){
   }
 
   try{
+    ROS_INFO("Started");
     OctomapServerStatic ms(mapFilename);
     ros::spin();
   }catch(std::runtime_error& e){