diff --git a/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/graphSlam3DGPU.hpp b/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/graphSlam3DGPU.hpp
index 4e82a0e816a41c672d240ee3ef10abb836335e0e..fabec04d2cc59be59b4c2e4316188896adca1603 100644
--- a/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/graphSlam3DGPU.hpp
+++ b/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/graphSlam3DGPU.hpp
@@ -65,6 +65,10 @@ private:
    //Only use points in icp alignment when they have a z normal
    //(normalized) greater than this
    double NormThresh;
+   //Eliminate surface points detected when the cell the surface lies on
+   //has not had a depth point detected in it. Helps remove incorrect surfaces
+   //when the camera goes on both sides of a narrow surface
+   bool UseOccupancyForSurface;
 
    //Derived params
    int NumBlocksTotal;
diff --git a/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/openclCommon.h b/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/openclCommon.h
index 75e673238517a0fe5e8f6bd2cf6a38d6da0110d0..3d1f921b98b3c6c5a12a6246088dc307f20ce72c 100644
--- a/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/openclCommon.h
+++ b/crosbot_3d_graphslam_gpu/include/crosbot_3d_graphslam_gpu/openclCommon.h
@@ -79,6 +79,9 @@ typedef struct {
    //Only points with a z normal greater than this will be used in
    //ICP alignment (0 disables)
    ocl_float NormThresh;
+   //Only detects a surface point if the cell the point is in is
+   //marked as occupied
+   ocl_int UseOccupancyForSurface;
 
 } oclGraphSlam3DConfig;
 
@@ -91,6 +94,7 @@ typedef struct {
    unsigned char r[NUM_CELLS];
    unsigned char g[NUM_CELLS];
    unsigned char b[NUM_CELLS];
+   unsigned char occupied[NUM_CELLS];
 #else
    ocl_float *distance;
    ocl_float *weight;
@@ -98,6 +102,7 @@ typedef struct {
    unsigned char *r;
    unsigned char *g;
    unsigned char *b;
+   unsigned char *occupied;
 #endif
 }  oclLocalBlock;
 
diff --git a/crosbot_3d_graphslam_gpu/launch/crosbot_3d_graphslam_gpu_board.launch b/crosbot_3d_graphslam_gpu/launch/crosbot_3d_graphslam_gpu_board.launch
index df43f622e4201d84bbf49dd62d28671f25341599..37f3df7e5bfcc66e0572ea6e960cdd0e8db3d55c 100644
--- a/crosbot_3d_graphslam_gpu/launch/crosbot_3d_graphslam_gpu_board.launch
+++ b/crosbot_3d_graphslam_gpu/launch/crosbot_3d_graphslam_gpu_board.launch
@@ -62,6 +62,8 @@
       <param name="CellSize" value="0.025" />
       <param name="LocalMapHeight" value="3" />
       <param name="MaxDistance" value="3" />
+      <!--param name="TruncNeg" value="0.1" /-->
+      <!--param name="TruncPos" value="0.15" /-->
 	</node>
 
    <node pkg="crosbot_3d_graphslam_display" type="crosbot_3d_graphslam_display" name="crosbot_3d_graphslam_display" output="screen" />
diff --git a/crosbot_3d_graphslam_gpu/src/graphSlam3DGPU.cpp b/crosbot_3d_graphslam_gpu/src/graphSlam3DGPU.cpp
index 0891290d8ba5747559f792de443a5934d6f0a5f1..c992caa5994dd97fad83be020cea54cd978393a1 100644
--- a/crosbot_3d_graphslam_gpu/src/graphSlam3DGPU.cpp
+++ b/crosbot_3d_graphslam_gpu/src/graphSlam3DGPU.cpp
@@ -84,6 +84,7 @@ void GraphSlam3DGPU::initialise(ros::NodeHandle &nh) {
    paramNH.param<int>("MinCount", MinCount, 300);
    paramNH.param<double>("MoveThresh", MoveThresh, 0.01);
    paramNH.param<double>("NormThresh", NormThresh, 0.8);
+   paramNH.param<bool>("UseOccupancyForSurface", UseOccupancyForSurface, true);
 
    NumBlocksWidth = (LocalMapWidth + 0.00001) / BlockSize;
    NumBlocksHeight = (LocalMapHeight + 0.00001) / BlockSize;
@@ -127,6 +128,7 @@ void GraphSlam3DGPU::initialiseGraphSlam(DepthPointsPtr depthPoints) {
    graphSlam3DConfig.MaxDistance = MaxDistance;
    graphSlam3DConfig.MaxSearchCells = MaxSearchDistance / CellSize;
    graphSlam3DConfig.NormThresh = NormThresh;
+   graphSlam3DConfig.UseOccupancyForSurface = UseOccupancyForSurface;
 
    clGraphSlam3DConfig = opencl_manager->deviceAlloc(sizeof(oclGraphSlam3DConfig),
          CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, &graphSlam3DConfig);
@@ -443,7 +445,7 @@ void GraphSlam3DGPU::initialiseLocalMap() {
    oclLocalBlock localBlock;
    size_t localBlockSize = (sizeof(*(localBlock.distance)) +
       sizeof(*(localBlock.weight)) + sizeof(*(localBlock.pI)) +
-      sizeof(*(localBlock.r)) * 3 + 1) * NumCellsTotal + sizeof(localBlock.blockIndex);
+      sizeof(*(localBlock.r)) * 4 + 1) * NumCellsTotal + sizeof(localBlock.blockIndex);
    clLocalMapCells = opencl_manager->deviceAlloc(localBlockSize * NumBlocksAllocated, 
          CL_MEM_READ_WRITE, NULL);
 
@@ -491,11 +493,12 @@ void GraphSlam3DGPU::checkBlocksExist(int numPoints, tf::Transform trans) {
    opencl_task->setArg(1, kernelI, sizeof(cl_mem), &clLocalMapBlocks);
    opencl_task->setArg(2, kernelI, sizeof(cl_mem), &clLocalMapCommon);
    opencl_task->setArg(3, kernelI, sizeof(cl_mem), &clPoints);
-   opencl_task->setArg(4, kernelI, sizeof(int), &numPoints);
-   opencl_task->setArg(5, kernelI, sizeof(ocl_float3), &clOrigin);
-   opencl_task->setArg(6, kernelI, sizeof(ocl_float3), &clBasis[0]);
-   opencl_task->setArg(7, kernelI, sizeof(ocl_float3), &clBasis[1]);
-   opencl_task->setArg(8, kernelI, sizeof(ocl_float3), &clBasis[2]);
+   opencl_task->setArg(4, kernelI, sizeof(cl_mem), &clLocalMapCells);
+   opencl_task->setArg(5, kernelI, sizeof(int), &numPoints);
+   opencl_task->setArg(6, kernelI, sizeof(ocl_float3), &clOrigin);
+   opencl_task->setArg(7, kernelI, sizeof(ocl_float3), &clBasis[0]);
+   opencl_task->setArg(8, kernelI, sizeof(ocl_float3), &clBasis[1]);
+   opencl_task->setArg(9, kernelI, sizeof(ocl_float3), &clBasis[2]);
 
    opencl_task->queueKernel(kernelI, 1, globalSize, LocalSize, 0, NULL, NULL, false);
 }
diff --git a/crosbot_3d_graphslam_gpu/src/opencl/graphSlam3d.cl b/crosbot_3d_graphslam_gpu/src/opencl/graphSlam3d.cl
index e0ac470bf7779dce757e345bc43754a8b8220faf..35d78042556f50b0d9fa7fa8556fa9d46e60e526 100644
--- a/crosbot_3d_graphslam_gpu/src/opencl/graphSlam3d.cl
+++ b/crosbot_3d_graphslam_gpu/src/opencl/graphSlam3d.cl
@@ -22,6 +22,7 @@ __kernel void clearLocalMap(constant oclGraphSlam3DConfig *config,
       localMapCells[blockIndex].r[cellIndex] = 0;
       localMapCells[blockIndex].g[cellIndex] = 0;
       localMapCells[blockIndex].b[cellIndex] = 0;
+      localMapCells[blockIndex].occupied[cellIndex] = 0;
       localMapCells[blockIndex].pI[cellIndex] = -1;
    }
 
@@ -156,7 +157,8 @@ int getCellIndex(constant oclGraphSlam3DConfig *config, float3 point, int bIndex
 }
 
 void markBlockActive(constant oclGraphSlam3DConfig *config, global int *blocks,
-      global oclLocalMapCommon *common, int bIndex) {
+      global oclLocalMapCommon *common, int bIndex, int isOrig, int cIndex,
+      global oclLocalBlock *localMapCells) {
    if (bIndex >= 0) {
       //Deal with the block
       if (blocks[bIndex] == -1) {
@@ -183,6 +185,14 @@ void markBlockActive(constant oclGraphSlam3DConfig *config, global int *blocks,
                atomic_xchg(&(blocks[bIndex]), oldVal % config->NumBlocksAllocated);
             } 
          }
+      } 
+      if (blocks[bIndex] >= 0 && isOrig && config->UseOccupancyForSurface) {
+         int blockI = blocks[bIndex] % config->NumBlocksAllocated;
+         if (localMapCells[blockI].occupied[cIndex] == 0) {
+            //Note: concurrency doesn't really matter here - as long as one
+            //add works
+            localMapCells[blockI].occupied[cIndex]++;
+         }
       }
    }
 }
@@ -193,7 +203,7 @@ void markBlockActive(constant oclGraphSlam3DConfig *config, global int *blocks,
  */
 __kernel void checkBlocksExist(constant oclGraphSlam3DConfig *config,
       global int *blocks, global oclLocalMapCommon *common,
-      global oclDepthPoints *points,
+      global oclDepthPoints *points, global oclLocalBlock *localMapCells,
       const int numPoints, const float3 origin,
       const float3 rotation0, const float3 rotation1, const float3 rotation2) {
 
@@ -209,7 +219,8 @@ __kernel void checkBlocksExist(constant oclGraphSlam3DConfig *config,
 
       int bIndex = getBlockIndex(config, transP);
       if (bIndex >= 0) {
-         markBlockActive(config, blocks, common, bIndex);
+         int cIndex = getCellIndex(config, transP, bIndex);
+         markBlockActive(config, blocks, common, bIndex, 1, cIndex, localMapCells);
          
          //todo: could find needed adjacent points by looking at the intersection of the ray with the block
 
@@ -235,35 +246,35 @@ __kernel void checkBlocksExist(constant oclGraphSlam3DConfig *config,
          }*/
 
          int adjXP = getBlockAdjX(config, bIndex, 1);
-         markBlockActive(config, blocks, common, adjXP);
+         markBlockActive(config, blocks, common, adjXP, 0, cIndex, localMapCells);
          int adjXN = getBlockAdjX(config, bIndex, -1);
-         markBlockActive(config, blocks, common, adjXN);
+         markBlockActive(config, blocks, common, adjXN, 0, cIndex, localMapCells);
          int adjYP = getBlockAdjX(config, bIndex, 1);
-         markBlockActive(config, blocks, common, adjYP);
+         markBlockActive(config, blocks, common, adjYP, 0, cIndex, localMapCells);
          int adjYN = getBlockAdjX(config, bIndex, -1);
-         markBlockActive(config, blocks, common, adjYN);
+         markBlockActive(config, blocks, common, adjYN, 0, cIndex, localMapCells);
          int adjZP = getBlockAdjX(config, bIndex, 1);
-         markBlockActive(config, blocks, common, adjZP);
+         markBlockActive(config, blocks, common, adjZP, 0, cIndex, localMapCells);
          int adjZN = getBlockAdjX(config, bIndex, -1);
-         markBlockActive(config, blocks, common, adjZN);
+         markBlockActive(config, blocks, common, adjZN, 0, cIndex, localMapCells);
          adjXP = getBlockAdjX(config, adjXP, 1);
-         markBlockActive(config, blocks, common, adjXP);
+         markBlockActive(config, blocks, common, adjXP, 0, cIndex, localMapCells);
          adjXN = getBlockAdjX(config, adjXN, -1);
-         markBlockActive(config, blocks, common, adjXN);
+         markBlockActive(config, blocks, common, adjXN, 0, cIndex, localMapCells);
          adjYP = getBlockAdjX(config, adjYP, 1);
-         markBlockActive(config, blocks, common, adjYP);
+         markBlockActive(config, blocks, common, adjYP, 0, cIndex, localMapCells);
          adjYN = getBlockAdjX(config, adjYN, -1);
-         markBlockActive(config, blocks, common, adjYN);
+         markBlockActive(config, blocks, common, adjYN, 0, cIndex, localMapCells);
          adjZP = getBlockAdjX(config, adjZP, 1);
-         markBlockActive(config, blocks, common, adjZP);
+         markBlockActive(config, blocks, common, adjZP, 0, cIndex, localMapCells);
          adjZN = getBlockAdjX(config, adjZN, -1);
-         markBlockActive(config, blocks, common, adjZN);
-         markBlockActive(config, blocks, common, getBlockAdjX(config, adjXP, 1));
-         markBlockActive(config, blocks, common, getBlockAdjX(config, adjXN, -1));
-         markBlockActive(config, blocks, common, getBlockAdjX(config, adjYP, 1));
-         markBlockActive(config, blocks, common, getBlockAdjX(config, adjYN, -1));
-         markBlockActive(config, blocks, common, getBlockAdjX(config, adjZP, 1));
-         markBlockActive(config, blocks, common, getBlockAdjX(config, adjZN, -1));
+         markBlockActive(config, blocks, common, adjZN, 0, cIndex, localMapCells);
+         markBlockActive(config, blocks, common, getBlockAdjX(config, adjXP, 1), 0, cIndex, localMapCells);
+         markBlockActive(config, blocks, common, getBlockAdjX(config, adjXN, -1), 0, cIndex, localMapCells);
+         markBlockActive(config, blocks, common, getBlockAdjX(config, adjYP, 1), 0, cIndex, localMapCells);
+         markBlockActive(config, blocks, common, getBlockAdjX(config, adjYN, -1), 0, cIndex, localMapCells);
+         markBlockActive(config, blocks, common, getBlockAdjX(config, adjZP, 1), 0, cIndex, localMapCells);
+         markBlockActive(config, blocks, common, getBlockAdjX(config, adjZN, -1), 0, cIndex, localMapCells);
          /*markBlockActive(config, blocks, common, getBlockAdjX(config, adjZP, 1));
          markBlockActive(config, blocks, common, getBlockAdjX(config, adjZP, -1));
          markBlockActive(config, blocks, common, getBlockAdjX(config, adjZN, 1));
@@ -572,14 +583,20 @@ void checkDirection(constant oclGraphSlam3DConfig *config, global oclLocalBlock
    for (int i = startI; count < config->NumCellsWidth; i += increment, count++) {
       nextI = i + increment;
       cellVal = localMapCells[bIndex].distance[i];
+      int bni = 0;
+      int ni = 0;
       if (count + 1 < config->NumCellsWidth) {
          cellNextVal = localMapCells[bIndex].distance[nextI];
+         bni = bIndex;
+         ni = nextI;
       } else if (bNextIndex < 0 || bNextIndex > config->NumBlocksAllocated) {
          continue;
       } else {
       //if (incYV > 0.5) { continue; }
       //continue;
          cellNextVal = localMapCells[bNextIndex].distance[startI];
+         bni = bNextIndex;
+         ni = startI;
       }
       if (isnan(cellVal) || isnan(cellNextVal)) {
          continue;
@@ -587,7 +604,9 @@ void checkDirection(constant oclGraphSlam3DConfig *config, global oclLocalBlock
       if (fabs(cellVal) > config->CellSize * 10 || fabs(cellNextVal) > config->CellSize * 10) {
          continue;
       }
-      if ((sign(cellVal) != sign(cellNextVal) || cellVal == 0) && localMapCells[bIndex].weight[i] > 5.0f) {
+      if ((sign(cellVal) != sign(cellNextVal) || cellVal == 0) && 
+            localMapCells[bIndex].weight[i] > 5.0f && (config->UseOccupancyForSurface == 0 || 
+            localMapCells[bIndex].occupied[i] > 0 || localMapCells[bni].occupied[ni] > 0)) {
          //There is a crossing!
          float3 p = getCellCentre(config, i, localMapCells[bIndex].blockIndex);
          float inc = fabs(cellVal / (cellNextVal - cellVal)) * config->CellSize;