diff --git a/octomap_server/src/OctomapServer.cpp b/octomap_server/src/OctomapServer.cpp
index 01eb19a2..59c58f7d 100644
--- a/octomap_server/src/OctomapServer.cpp
+++ b/octomap_server/src/OctomapServer.cpp
@@ -260,24 +260,29 @@ bool OctomapServer::openFile(const std::string& filename){
 
 void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
   ros::WallTime startTime = ros::WallTime::now();
-
-
   //
   // ground filtering in base frame
   //
   PCLPointCloud pc; // input cloud for filtering and ground-detection
   pcl::fromROSMsg(*cloud, pc);
 
-  tf::StampedTransform sensorToWorldTf;
+  tf::StampedTransform sensorToWorldTf, sensorToBaseTf, baseToWorldTf;
   try {
     m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
+    m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2));
+    m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf);
+    m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf);
   } catch(tf::TransformException& ex){
-    ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
+    ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback.\n"
+                      "You need to set base_frame_id to apply filtering.\n"
+                      "Also check if transforms exist between map, sensor and base frames.");
     return;
   }
 
-  Eigen::Matrix4f sensorToWorld;
+  Eigen::Matrix4f sensorToWorld, sensorToBase, baseToWorld;
   pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
+  pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase);
+  pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld);
 
 
   // set up filter for height range, also removes NANs:
@@ -294,55 +299,28 @@ void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr
   PCLPointCloud pc_ground; // segmented ground plane
   PCLPointCloud pc_nonground; // everything else
 
+  // transform pointcloud from sensor frame to fixed robot frame
+  pcl::transformPointCloud(pc, pc, sensorToBase);
+  pass_x.setInputCloud(pc.makeShared());
+  pass_x.filter(pc);
+  pass_y.setInputCloud(pc.makeShared());
+  pass_y.filter(pc);
+  
   if (m_filterGroundPlane){
-    tf::StampedTransform sensorToBaseTf, baseToWorldTf;
-    try{
-      m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2));
-      m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf);
-      m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf);
-
-
-    }catch(tf::TransformException& ex){
-      ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n"
-                        "You need to set the base_frame_id or disable filter_ground.");
-    }
-
-
-    Eigen::Matrix4f sensorToBase, baseToWorld;
-    pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase);
-    pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld);
-
-    // transform pointcloud from sensor frame to fixed robot frame
-    pcl::transformPointCloud(pc, pc, sensorToBase);
-    pass_x.setInputCloud(pc.makeShared());
-    pass_x.filter(pc);
-    pass_y.setInputCloud(pc.makeShared());
-    pass_y.filter(pc);
     pass_z.setInputCloud(pc.makeShared());
     pass_z.filter(pc);
     filterGroundPlane(pc, pc_ground, pc_nonground);
-
-    // transform clouds to world frame for insertion
-    pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld);
-    pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld);
-  } else {
-    // directly transform to map frame:
-    pcl::transformPointCloud(pc, pc, sensorToWorld);
-
-    // just filter height range:
-    pass_x.setInputCloud(pc.makeShared());
-    pass_x.filter(pc);
-    pass_y.setInputCloud(pc.makeShared());
-    pass_y.filter(pc);
+  } 
+  else {
     pass_z.setInputCloud(pc.makeShared());
-    pass_z.filter(pc);
-
-    pc_nonground = pc;
-    // pc_nonground is empty without ground segmentation
-    pc_ground.header = pc.header;
-    pc_nonground.header = pc.header;
+    pass_z.filter(pc_nonground);
+    pass_z.setFilterLimitsNegative(true);
+    pass_z.filter(pc_ground);
   }
 
+  // transform clouds to world frame for insertion
+  pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld);
+  pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld);
 
   insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground);