Add snapping kernels for CUDA to the RapidHeightMapExtractorCUDA #616
Conversation
…ult for real robot
…ill causing problems, but runs without errors to console.
…p and removing the bounds chesk cause it happens at the top of the kernel
…wnloading data from the gpu?
| @@ -66,7 +66,7 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, ROS2Helper ros2Helper, ROS2 | |||
|
|
|||
| private void initializeHeightMap() | |||
| { | |||
| boolean runWithCUDA = false; | |||
| boolean runWithCUDA = true; | |||
There was a problem hiding this comment.
When running on the real robot we try to use CUDA as the default now
| openCLManager.setKernelArgument(computeSnappedValuesKernel, 0, snappingParametersBuffer.getOpenCLBufferObject()); | ||
| openCLManager.setKernelArgument(computeSnappedValuesKernel, 1, globalHeightMapImage.getOpenCLImageObject()); | ||
| openCLManager.setKernelArgument(computeSnappedValuesKernel, 2, yaw.getOpenCLBufferObject()); |
There was a problem hiding this comment.
This yaw parameter is never used, removed it
|
|
||
| bool should_print = false;//idx_x == 20 && idx_y == 20; | ||
|
|
||
| int2 key = (int2) (idx_x, idx_y); | ||
| uint foot_height_int = read_imageui(height_map, key).x; |
There was a problem hiding this comment.
All unused code in the kernel
| continue; | ||
|
|
||
| float snap_height_threshold = params[MIN_SNAP_HEIGHT_THRESHOLD] + params[SNAP_HEIGHT_THRESHOLD_AT_SEARCH_EDGE] * clamp(offset_distance / foot_search_radius, 0.0f, 1.0f); | ||
| float min_height_under_foot_to_consider = max_height_under_foot - snap_height_threshold; | ||
|
|
||
| if (isnan(query_height)) // || query_height < min_height_under_foot_to_consider) |
There was a problem hiding this comment.
We check this above, and the query_height value doesn't get updated between the previous isnan(query_height) call and this one, removed it
| @@ -438,7 +438,6 @@ extern "C" __global__ void heightMapRegistrationKernel(unsigned short *localMap, | |||
| { | |||
| finalHeight = localHeight; | |||
| } | |||
| finalHeight = get_spatial_filtered_height(xIndex, yIndex, finalHeight, globalMap, params); | |||
There was a problem hiding this comment.
For whatever reason, this filter was causing the g;obal height map to have a strange pattern in it that wasn't flat. @PotatoPeeler3000 Need to look into this again, not sure this is the correct move.
| @@ -142,66 +196,6 @@ public void recomputeDerivedParameters() | |||
| throw new RuntimeException("The crop center index was computed incorrectly."); | |||
| } | |||
|
|
|||
| public void initialize() throws Exception | |||
There was a problem hiding this comment.
Initialize should just happen in the constructor, changed
| @@ -216,6 +210,7 @@ public void reset() | |||
|
|
|||
| localHeightMapImage.setTo(new Scalar(offset)); | |||
| globalHeightMapImage.setTo(new Scalar(offset)); | |||
| snapHeightImage.setTo(new Scalar(32768)); | |||
There was a problem hiding this comment.
Don't know why this has a hard coded value, its pulled from the OpenCL kernel
There was a problem hiding this comment.
More like the extractor, in java land they use this same number on the rest to set the snapped height map too
| // Rect roi = new Rect(0, 0, 151, 151); | ||
| // Mat croppedMat = new Mat(finalCroppedHeightMap, roi); | ||
| // terrainMapData.setHeightMap(croppedMat); | ||
| Mat cpuSnapHeightMap = new Mat(); |
There was a problem hiding this comment.
All the mats to set the TerrainMapData
| // enables a better "sharp" edge around corners, to avoid rounding by averaging. It's also how the support area is calculated. In the future, the support area | ||
| // should be the area of the convex hull, not just the area of the cells, since that will allow "bridging" gaps. | ||
| extern "C" | ||
| __global__ void computeSnappedValuesKernel(unsigned short *globalMap, size_t pitchGlobal, |
There was a problem hiding this comment.
Here she is, massive
| } | ||
|
|
||
| extern "C" | ||
| __global__ void computeSteppabilityConnectionsKernel(float* params, |
There was a problem hiding this comment.
Second kernel that currently isn't used but may be added to Java stuff in the future
When using the footstep planner we have the ability to snap the footsteps to the height map. The computation for this is done on the GPU with the snapping kernel. We get several different Mat()'s from the GPU that has different data in regards to snapping. This allows for this to happen very quickly and not slow down the planner when its trying to snap the feet.