From 79bf008e458e5e63d9a030f983f490866f223248 Mon Sep 17 00:00:00 2001 From: adrianrobolab Date: Thu, 4 Dec 2014 18:26:17 +1100 Subject: [PATCH] Added some filtering of surface points for 3d map construction to use occupancy status of cells to remove surface artefacts --- .../graphSlam3DGPU.hpp | 4 ++ .../crosbot_3d_graphslam_gpu/openclCommon.h | 5 ++ .../crosbot_3d_graphslam_gpu_board.launch | 2 + .../src/graphSlam3DGPU.cpp | 15 +++-- .../src/opencl/graphSlam3d.cl | 63 ++++++++++++------- 5 files changed, 61 insertions(+), 28 deletions(-) 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 4e82a0e..fabec04 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 75e6732..3d1f921 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 df43f62..37f3df7 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 @@ + + diff --git a/crosbot_3d_graphslam_gpu/src/graphSlam3DGPU.cpp b/crosbot_3d_graphslam_gpu/src/graphSlam3DGPU.cpp index 0891290..c992caa 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("MinCount", MinCount, 300); paramNH.param("MoveThresh", MoveThresh, 0.01); paramNH.param("NormThresh", NormThresh, 0.8); + paramNH.param("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 e0ac470..35d7804 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; -- GitLab