From f4cb234a1b3b60204fb1e946e9694b42b2d47d26 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Mon, 10 Feb 2025 20:45:21 -0600 Subject: [PATCH 01/10] Added an example of how to create a 3d matrix --- .../cuda/examples/CUDAExampleKernel3D.java | 102 ++++++++++++++++++ .../perception/cuda/examples/matrix_3d.cu | 27 +++++ 2 files changed, 129 insertions(+) create mode 100644 ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java create mode 100644 ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java new file mode 100644 index 00000000000..a5196feb92f --- /dev/null +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -0,0 +1,102 @@ +package us.ihmc.perception.cuda.examples; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.cuda.global.cudart; +import org.bytedeco.javacpp.ShortPointer; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import us.ihmc.perception.cuda.CUDAKernel; +import us.ihmc.perception.cuda.CUDAProgram; +import us.ihmc.perception.cuda.CUDAStreamManager; +import us.ihmc.perception.cuda.CUDATools; + +import java.net.URL; +import java.util.ArrayList; +import java.util.List; + +import static org.bytedeco.cuda.global.cudart.*; + +public class CUDAExampleKernel3D +{ + public CUDAExampleKernel3D() throws Exception + { + int width = 2; + int height = 2; + int layers = 2; + + CUstream_st stream = CUDAStreamManager.getStream(); + + GpuMat gpuMatExample = new GpuMat(height, width, opencv_core.CV_16UC1); + long pitchA = gpuMatExample.step(); // CUDA pitch (in bytes) + + // Allocate enough memory for all layers + ShortPointer heightMapHistory = new ShortPointer(); + cudaMalloc(heightMapHistory, layers * pitchA * height); + int error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + URL programPath = getClass().getResource("matrix_3d.cu"); + String kernelName = "matrix_3d_example"; + + + Mat cpuData1 = new Mat(height, width, opencv_core.CV_16UC1); + cpuData1.ptr(0, 0, 1); + Mat cpuData2 = new Mat(height, width, opencv_core.CV_16UC1); + cpuData2.ptr(0, 0, 2); + + List heightMaps = new ArrayList<>(); + heightMaps.add(cpuData1); + heightMaps.add(cpuData2); + + List gpuHeightMaps = new ArrayList<>(); + + for (int i = 0; i < layers; i++) + { + Mat cpuData = new Mat(height, width, opencv_core.CV_16UC1, new Scalar(i + 1)); +// cpuData.setTo(new Scalar(i + 1)); // Example: Fill with 1 for layer 0, 2 for layer 1 + + GpuMat gpuData = new GpuMat(); + gpuData.upload(cpuData); + + // Copy this layer into the allocated memory + cudaMemcpy2D(heightMapHistory.position(i * pitchA * height), pitchA, + gpuData.data(), gpuData.step(), + width * Short.BYTES, height, + cudaMemcpyHostToDevice); + } + + CUDAProgram program = new CUDAProgram(programPath); + CUDAKernel kernel = program.loadKernel(kernelName); + + kernel.withPointer(heightMapHistory); + kernel.withLong(pitchA); + kernel.withLong(pitchA * height); // layerSize = pitchA * height + kernel.withInt(height); + kernel.withInt(width); + kernel.withInt(layers); + + dim3 gridDim = new dim3(); // The same thing as ( new dim3(1, 1, 1); ) + dim3 blockDim = new dim3(width, height, 1); + kernel.run(stream, gridDim, blockDim, 0); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + cudaStreamSynchronize(stream); + + kernel.close(); + program.close(); + gridDim.close(); + blockDim.close(); + + CUDAStreamManager.releaseStream(stream); + } + + public static void main(String[] args) throws Exception + { + new CUDAExampleKernel3D(); + } +} diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu new file mode 100644 index 00000000000..c287ba56ae0 --- /dev/null +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -0,0 +1,27 @@ +extern "C" +__global__ +void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, size_t layerSize, int rows, int cols, int layers) +{ + int indexX = blockIdx.x * blockDim.x + threadIdx.x; // Column index + int indexY = blockIdx.y * blockDim.y + threadIdx.y; // Row index + + if (indexX >= cols || indexY >= rows) + return; // Prevent out-of-bounds access + + // Loop over layers + for (int layer = 0; layer < layers; layer++) + { + // Compute the base address of the current layer + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + + // Compute row offset using pitchA + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + // Fetch value from the current layer + int query_height_int = (int)(*matrixPointerRow); + + // Print out the value along with its position in the 3D matrix + printf("Layer %d, Value at (%d, %d): %d\n", layer, indexX, indexY, query_height_int); + } +} + From a8294a643418ed5abb7107f3472d83352bbacaea Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Mon, 10 Feb 2025 21:25:50 -0600 Subject: [PATCH 02/10] Remove unused code for testing --- .../cuda/examples/CUDAExampleKernel3D.java | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index a5196feb92f..a3d97582b5a 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -2,7 +2,6 @@ import org.bytedeco.cuda.cudart.CUstream_st; import org.bytedeco.cuda.cudart.dim3; -import org.bytedeco.cuda.global.cudart; import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; @@ -14,8 +13,6 @@ import us.ihmc.perception.cuda.CUDATools; import java.net.URL; -import java.util.ArrayList; -import java.util.List; import static org.bytedeco.cuda.global.cudart.*; @@ -41,22 +38,9 @@ public CUDAExampleKernel3D() throws Exception URL programPath = getClass().getResource("matrix_3d.cu"); String kernelName = "matrix_3d_example"; - - Mat cpuData1 = new Mat(height, width, opencv_core.CV_16UC1); - cpuData1.ptr(0, 0, 1); - Mat cpuData2 = new Mat(height, width, opencv_core.CV_16UC1); - cpuData2.ptr(0, 0, 2); - - List heightMaps = new ArrayList<>(); - heightMaps.add(cpuData1); - heightMaps.add(cpuData2); - - List gpuHeightMaps = new ArrayList<>(); - for (int i = 0; i < layers; i++) { Mat cpuData = new Mat(height, width, opencv_core.CV_16UC1, new Scalar(i + 1)); -// cpuData.setTo(new Scalar(i + 1)); // Example: Fill with 1 for layer 0, 2 for layer 1 GpuMat gpuData = new GpuMat(); gpuData.upload(cpuData); From 7f03cdaca8b5c42e8b39184da0a69fd82ff1663d Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 11 Feb 2025 09:27:25 -0600 Subject: [PATCH 03/10] Fixed 3d cuda example and return the average to the cpu --- .../cuda/examples/CUDAExampleKernel3D.java | 79 +++++++++++++------ .../perception/cuda/examples/matrix_3d.cu | 25 +++--- 2 files changed, 70 insertions(+), 34 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index a3d97582b5a..e47f0120556 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -11,71 +11,102 @@ import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.cuda.CUDATools; +import us.ihmc.perception.tools.PerceptionDebugTools; import java.net.URL; +import java.util.ArrayList; +import java.util.List; import static org.bytedeco.cuda.global.cudart.*; public class CUDAExampleKernel3D { + /** + * This is an example of how to pass in a 3d array into the CUDA kernel. + * Each index will have values in each layer. + * This averages each index's layer and returns the result in a {@link GpuMat}. + */ public CUDAExampleKernel3D() throws Exception { - int width = 2; - int height = 2; - int layers = 2; + // Create a 2x2 with 5 layers + int rows = 2; + int cols = 2; + int layers = 4; CUstream_st stream = CUDAStreamManager.getStream(); - GpuMat gpuMatExample = new GpuMat(height, width, opencv_core.CV_16UC1); - long pitchA = gpuMatExample.step(); // CUDA pitch (in bytes) + URL programPath = getClass().getResource("matrix_3d.cu"); + String kernelName = "matrix_3d_example"; + CUDAProgram program = new CUDAProgram(programPath); + CUDAKernel kernel = program.loadKernel(kernelName); + + GpuMat gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); + long pitchForLayer = gpuMatExample.step(); // Allocate enough memory for all layers - ShortPointer heightMapHistory = new ShortPointer(); - cudaMalloc(heightMapHistory, layers * pitchA * height); + ShortPointer pointerTo3DArray = new ShortPointer(); + cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); int error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - URL programPath = getClass().getResource("matrix_3d.cu"); - String kernelName = "matrix_3d_example"; + List gpuLayers = new ArrayList<>(); for (int i = 0; i < layers; i++) { - Mat cpuData = new Mat(height, width, opencv_core.CV_16UC1, new Scalar(i + 1)); + // To get the average to be a whole number do (i * 2 + 2) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); + // Upload that data to the gpu so we can allocate the memory for it GpuMat gpuData = new GpuMat(); gpuData.upload(cpuData); - // Copy this layer into the allocated memory - cudaMemcpy2D(heightMapHistory.position(i * pitchA * height), pitchA, - gpuData.data(), gpuData.step(), - width * Short.BYTES, height, + // Allocate memory for each layer, creating a 3d array + cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), + pitchForLayer, + gpuData.data(), + gpuData.step(), + cols * Short.BYTES, + rows, cudaMemcpyHostToDevice); + + // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end + gpuLayers.add(gpuData); } - CUDAProgram program = new CUDAProgram(programPath); - CUDAKernel kernel = program.loadKernel(kernelName); + GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - kernel.withPointer(heightMapHistory); - kernel.withLong(pitchA); - kernel.withLong(pitchA * height); // layerSize = pitchA * height - kernel.withInt(height); - kernel.withInt(width); + kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(result.data()).withLong(result.step()); + kernel.withLong(pitchForLayer * rows); + kernel.withInt(rows); + kernel.withInt(cols); kernel.withInt(layers); - dim3 gridDim = new dim3(); // The same thing as ( new dim3(1, 1, 1); ) - dim3 blockDim = new dim3(width, height, 1); + dim3 gridDim = new dim3(); + dim3 blockDim = new dim3(cols, rows, 1); kernel.run(stream, gridDim, blockDim, 0); error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - cudaStreamSynchronize(stream); + // Print the result to make sure we get what we expect + // In this example we go through 2,4,6,8 so we expect the average to be 5 + Mat cpuResult = new Mat(); + result.download(cpuResult); + PerceptionDebugTools.printMat("Result", cpuResult, 1); kernel.close(); program.close(); gridDim.close(); blockDim.close(); + // This is where we close the individual gpu layers + for (int i = 0; i < layers; i++) + { + gpuLayers.get(i).release(); + gpuLayers.get(i).close(); + } + CUDAStreamManager.releaseStream(stream); } diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu index c287ba56ae0..baf164f8a48 100644 --- a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -1,14 +1,17 @@ extern "C" __global__ -void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, size_t layerSize, int rows, int cols, int layers) +void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, + unsigned short * resultPointer, size_t pitchResult, + size_t layerSize, int rows, int cols, int layers) { - int indexX = blockIdx.x * blockDim.x + threadIdx.x; // Column index - int indexY = blockIdx.y * blockDim.y + threadIdx.y; // Row index + int indexX = blockIdx.x * blockDim.x + threadIdx.x; + int indexY = blockIdx.y * blockDim.y + threadIdx.y; if (indexX >= cols || indexY >= rows) - return; // Prevent out-of-bounds access + return; + + int sum = 0; - // Loop over layers for (int layer = 0; layer < layers; layer++) { // Compute the base address of the current layer @@ -17,11 +20,13 @@ void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, size_t lay // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - // Fetch value from the current layer - int query_height_int = (int)(*matrixPointerRow); - - // Print out the value along with its position in the 3D matrix - printf("Layer %d, Value at (%d, %d): %d\n", layer, indexX, indexY, query_height_int); + sum += (int) *matrixPointerRow; } + + unsigned short avg = sum / layers; + + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; + *outputPointer = avg; + printf("GPU Average: %d, ", avg); } From c5b1f13002940d02d1f1bb53520a48e337a5d93c Mon Sep 17 00:00:00 2001 From: Tomasz Bialek Date: Tue, 11 Feb 2025 11:10:12 -0600 Subject: [PATCH 04/10] Using cudaMalloc3D --- .../cuda/examples/CUDAExampleKernel3D.java | 44 +++++++------------ .../perception/cuda/examples/matrix_3d.cu | 1 + 2 files changed, 16 insertions(+), 29 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index e47f0120556..0c22ac6dbf3 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -1,6 +1,8 @@ package us.ihmc.perception.cuda.examples; import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.cudaExtent; +import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; @@ -40,44 +42,35 @@ public CUDAExampleKernel3D() throws Exception CUDAProgram program = new CUDAProgram(programPath); CUDAKernel kernel = program.loadKernel(kernelName); - GpuMat gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); - long pitchForLayer = gpuMatExample.step(); - // Allocate enough memory for all layers - ShortPointer pointerTo3DArray = new ShortPointer(); - cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); - int error = cudaStreamSynchronize(stream); + cudaPitchedPtr pointerTo3DArray = new cudaPitchedPtr(); + cudaExtent extent = make_cudaExtent(cols * Short.BYTES, rows, layers); + int error = cudaMalloc3D(pointerTo3DArray, extent); CUDATools.checkCUDAError(error); - List gpuLayers = new ArrayList<>(); - for (int i = 0; i < layers; i++) { // To get the average to be a whole number do (i * 2 + 2) Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); - // Upload that data to the gpu so we can allocate the memory for it - GpuMat gpuData = new GpuMat(); - gpuData.upload(cpuData); - // Allocate memory for each layer, creating a 3d array - cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), - pitchForLayer, - gpuData.data(), - gpuData.step(), - cols * Short.BYTES, - rows, - cudaMemcpyHostToDevice); + cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + cpuData.data(), + cpuData.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end - gpuLayers.add(gpuData); + cpuData.close(); } GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); - kernel.withLong(pitchForLayer * rows); + kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); @@ -100,13 +93,6 @@ public CUDAExampleKernel3D() throws Exception gridDim.close(); blockDim.close(); - // This is where we close the individual gpu layers - for (int i = 0; i < layers; i++) - { - gpuLayers.get(i).release(); - gpuLayers.get(i).close(); - } - CUDAStreamManager.releaseStream(stream); } diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu index baf164f8a48..cf8a5bb23bb 100644 --- a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -20,6 +20,7 @@ void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } From 06c1006a754a97b57d0bfb95feb02674ea758992 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 11 Feb 2025 13:05:39 -0600 Subject: [PATCH 05/10] Started writing a test to have this example be used in a class --- .../FilteredRapidHeightMapExtractor.java | 143 ++++++++++++++++++ .../FilteredRapidHeightMapExtractor.cu | 32 ++++ .../FilteredRapidHeightMapExtractorTest.java | 36 +++++ 3 files changed, 211 insertions(+) create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java create mode 100644 ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu create mode 100644 ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java new file mode 100644 index 00000000000..7825ab424fe --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -0,0 +1,143 @@ +package us.ihmc.perception.gpuHeightMap; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.javacpp.ShortPointer; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import us.ihmc.perception.cuda.CUDAKernel; +import us.ihmc.perception.cuda.CUDAProgram; +import us.ihmc.perception.cuda.CUDATools; +import us.ihmc.perception.tools.PerceptionDebugTools; + +import java.net.URL; +import java.util.ArrayList; +import java.util.List; + +import static org.bytedeco.cuda.global.cudart.*; + +public class FilteredRapidHeightMapExtractor +{ + private final List gpuLayers; + private final ShortPointer pointerTo3DArray; + private final long pitchForLayer; + private final GpuMat gpuMatExample; + private int currentIndex; + int layers = 2; + + private final CUstream_st stream; + private final int rows; + private final int cols; + private final CUDAKernel kernel; + private final CUDAProgram program; + private int loopTracker = 0; + + public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) + { + this.stream = stream; + this.rows = rows; + this.cols = cols; + + // Load header and main file + URL kernelPath = getClass().getResource("FilteredRapidHeightMapExtractor.cu"); + try + { + program = new CUDAProgram(kernelPath); + kernel = program.loadKernel("filterRapidHeightMap"); + } + catch (Exception e) + { + throw new RuntimeException(e); + } + + gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); + pitchForLayer = gpuMatExample.step(); + + pointerTo3DArray = new ShortPointer(); + cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); + int error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + gpuLayers = new ArrayList<>(layers); + currentIndex = 0; + + for (int i = 0; i < layers; i++) + { + // To get the average to be a whole number do (i * 2 + 2) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(0)); + + // Upload that data to the gpu so we can allocate the memory for it + GpuMat gpuData = new GpuMat(); + gpuData.upload(cpuData); + + // Allocate memory for each layer, creating a 3d array + cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), + pitchForLayer, + gpuData.data(), + gpuData.step(), + cols * Short.BYTES, + rows, + cudaMemcpyHostToDevice); + + // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end + gpuLayers.add(gpuData); + } + } + + public GpuMat update(GpuMat latestGlobalHeightMap) + { + // Only want to compute the average if we have the past values to use + if (loopTracker < layers) + { + loopTracker++; + latestGlobalHeightMap.convertTo(gpuLayers.get(currentIndex), latestGlobalHeightMap.type()); + Mat temp = new Mat(); + gpuLayers.get(currentIndex).download(temp); + PerceptionDebugTools.printMat("s", temp, 1); + currentIndex = (currentIndex + 1) % layers; + return latestGlobalHeightMap; + } + + int error; + + GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); + + kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(result.data()).withLong(result.step()); + kernel.withLong(pitchForLayer * rows); + kernel.withInt(rows); + kernel.withInt(cols); + kernel.withInt(layers); + + dim3 gridDim = new dim3(); + dim3 blockDim = new dim3(cols, rows, 1); + kernel.run(stream, gridDim, blockDim, 0); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + // Print the result to make sure we get what we expect + // In this example we go through 2,4,6,8 so we expect the average to be 5 + Mat cpuResult = new Mat(); + result.download(cpuResult); + PerceptionDebugTools.printMat("Result", cpuResult, 1); + + gpuLayers.get(currentIndex).setTo(new Scalar(1), latestGlobalHeightMap); + currentIndex = (currentIndex + 1) % layers; + + return result; + } + + public void reset() + { + loopTracker = 0; + } + + public void close() + { + program.close(); + kernel.close(); + } +} diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu new file mode 100644 index 00000000000..7d4faba4595 --- /dev/null +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -0,0 +1,32 @@ +extern "C" +__global__ +void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, + unsigned short * resultPointer, size_t pitchResult, + size_t layerSize, int rows, int cols, int layers) +{ + int indexX = blockIdx.x * blockDim.x + threadIdx.x; + int indexY = blockIdx.y * blockDim.y + threadIdx.y; + + if (indexX >= cols || indexY >= rows) + return; + + int sum = 0; + + for (int layer = 0; layer < layers; layer++) + { + // Compute the base address of the current layer + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + + // Compute row offset using pitchA + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + printf("Layer: %d, Value: %d, %d, and: %d\n", layer, indexY, indexX, (int) *matrixPointerRow); + sum += (int) *matrixPointerRow; + } + + unsigned short avg = sum / layers; + + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; + *outputPointer = avg; +} + diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java new file mode 100644 index 00000000000..e1c0cc47854 --- /dev/null +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -0,0 +1,36 @@ +package us.ihmc.perception.gpuHeightMap; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import org.junit.jupiter.api.Test; +import us.ihmc.perception.cuda.CUDAStreamManager; +import us.ihmc.perception.tools.PerceptionDebugTools; + +public class FilteredRapidHeightMapExtractorTest +{ + @Test + public void testGettingAverage() + { + int rows = 2; + int cols = 2; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + } + + filteredRapidHeightMapExtractor.close(); + } +} From 94dac473e928601e5f25b31776ae8c2ff959c2bb Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 11 Feb 2025 13:58:20 -0600 Subject: [PATCH 06/10] Fixed FilteredRapidHeightMapExtractor to compute the average of the history of height maps --- .../FilteredRapidHeightMapExtractor.java | 76 ++++++++++--------- .../FilteredRapidHeightMapExtractor.cu | 3 +- 2 files changed, 43 insertions(+), 36 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 7825ab424fe..6349f6c5bc4 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -1,29 +1,26 @@ package us.ihmc.perception.gpuHeightMap; import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.cudaExtent; +import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; -import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; import org.bytedeco.opencv.opencv_core.Scalar; +import org.jetbrains.annotations.NotNull; import us.ihmc.perception.cuda.CUDAKernel; import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.tools.PerceptionDebugTools; import java.net.URL; -import java.util.ArrayList; -import java.util.List; import static org.bytedeco.cuda.global.cudart.*; public class FilteredRapidHeightMapExtractor { - private final List gpuLayers; - private final ShortPointer pointerTo3DArray; - private final long pitchForLayer; - private final GpuMat gpuMatExample; + private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; int layers = 2; @@ -52,15 +49,11 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) throw new RuntimeException(e); } - gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); - pitchForLayer = gpuMatExample.step(); - - pointerTo3DArray = new ShortPointer(); - cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); - int error = cudaStreamSynchronize(stream); + pointerTo3DArray = new cudaPitchedPtr(); + cudaExtent extent = make_cudaExtent((long) cols * Short.BYTES, rows, layers); + int error = cudaMalloc3D(pointerTo3DArray, extent); CUDATools.checkCUDAError(error); - gpuLayers = new ArrayList<>(layers); currentIndex = 0; for (int i = 0; i < layers; i++) @@ -68,34 +61,40 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) // To get the average to be a whole number do (i * 2 + 2) Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(0)); - // Upload that data to the gpu so we can allocate the memory for it - GpuMat gpuData = new GpuMat(); - gpuData.upload(cpuData); - // Allocate memory for each layer, creating a 3d array - cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), - pitchForLayer, - gpuData.data(), - gpuData.step(), - cols * Short.BYTES, - rows, - cudaMemcpyHostToDevice); - - // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end - gpuLayers.add(gpuData); + cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + cpuData.data(), + cpuData.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); } } public GpuMat update(GpuMat latestGlobalHeightMap) + { + GpuMat heightMapAverage = computerHeightMapHistoryAverage(latestGlobalHeightMap); + + return latestGlobalHeightMap; + } + + @NotNull + private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) { // Only want to compute the average if we have the past values to use if (loopTracker < layers) { loopTracker++; - latestGlobalHeightMap.convertTo(gpuLayers.get(currentIndex), latestGlobalHeightMap.type()); - Mat temp = new Mat(); - gpuLayers.get(currentIndex).download(temp); - PerceptionDebugTools.printMat("s", temp, 1); + + cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + currentIndex = (currentIndex + 1) % layers; return latestGlobalHeightMap; } @@ -104,9 +103,9 @@ public GpuMat update(GpuMat latestGlobalHeightMap) GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); - kernel.withLong(pitchForLayer * rows); + kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); @@ -124,7 +123,14 @@ public GpuMat update(GpuMat latestGlobalHeightMap) result.download(cpuResult); PerceptionDebugTools.printMat("Result", cpuResult, 1); - gpuLayers.get(currentIndex).setTo(new Scalar(1), latestGlobalHeightMap); + cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + currentIndex = (currentIndex + 1) % layers; return result; diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 7d4faba4595..1a4496deb55 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -20,7 +20,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - printf("Layer: %d, Value: %d, %d, and: %d\n", layer, indexY, indexX, (int) *matrixPointerRow); + printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } @@ -28,5 +28,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; *outputPointer = avg; + printf("GPU Average: %d, ", avg); } From 29920f7e817528fbc8e94ad97aaa103bed3767c2 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Tue, 11 Feb 2025 20:59:49 -0600 Subject: [PATCH 07/10] Updated the kernel to compute the height estimate based on a moving average and variance --- .../FilteredRapidHeightMapExtractor.java | 8 ++- .../FilteredRapidHeightMapExtractor.cu | 55 ++++++++++++++++--- .../FilteredRapidHeightMapExtractorTest.java | 2 +- 3 files changed, 55 insertions(+), 10 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 6349f6c5bc4..1af1ed7297b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -30,6 +30,7 @@ public class FilteredRapidHeightMapExtractor private final CUDAKernel kernel; private final CUDAProgram program; private int loopTracker = 0; + private int defaultValue; public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) { @@ -72,8 +73,10 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) } } - public GpuMat update(GpuMat latestGlobalHeightMap) + public GpuMat update(GpuMat latestGlobalHeightMap, int defaultValue) { + this.defaultValue = defaultValue; + GpuMat heightMapAverage = computerHeightMapHistoryAverage(latestGlobalHeightMap); return latestGlobalHeightMap; @@ -102,13 +105,16 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) int error; GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); + GpuMat variance = new GpuMat(rows, cols, opencv_core.CV_16UC1); kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); + kernel.withPointer(variance.data()).withLong(variance.step()); kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); + kernel.withInt(defaultValue); dim3 gridDim = new dim3(); dim3 blockDim = new dim3(cols, rows, 1); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 1a4496deb55..1b8457ee913 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,8 +1,11 @@ +#define LAYERS 2 // Set a fixed size + extern "C" __global__ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * resultPointer, size_t pitchResult, - size_t layerSize, int rows, int cols, int layers) + unsigned short * newestHeightMap, size_t pitchNewestHeightMap, + size_t layerSize, int rows, int cols, int defaultValue) { int indexX = blockIdx.x * blockDim.x + threadIdx.x; int indexY = blockIdx.y * blockDim.y + threadIdx.y; @@ -11,23 +14,59 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, return; int sum = 0; + float variance[LAYERS] = {0}; - for (int layer = 0; layer < layers; layer++) + // Compute the average height of the history in order to get the variance at each layer + for (int layer = 0; layer < LAYERS; layer++) { - // Compute the base address of the current layer unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); - - // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } - unsigned short avg = sum / layers; + unsigned short avg = sum / LAYERS; + + // Compute the variance for each layer + for (int layer = 0; layer < LAYERS; layer++) + { + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + float diff = (float)(*matrixPointerRow) - avg; + variance[layer] = diff * diff; + } + + double heightSum = 0; + double varianceSum = 0; + + // Compute the height and variance sum + for (int layer = 0; layer < LAYERS; layer++) + { + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + heightSum += (float)(*matrixPointerRow) / variance[layer]; + varianceSum += 1.0 / variance[layer]; + } + + unsigned short newHeight = heightSum / varianceSum; + + + unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; + float diff = (float) (*heightValue) - avg; + double newVariance = diff * diff; + + if (*heightValue == defaultValue) + return; + + unsigned short heightEstimate = (unsigned short) *heightValue + (newHeight * newVariance) / newVariance; + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; - *outputPointer = avg; - printf("GPU Average: %d, ", avg); + *outputPointer = heightEstimate; + + printf("GPU Height Estimate: %d\n", heightEstimate); } diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java index e1c0cc47854..c633eeaf563 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -25,7 +25,7 @@ public void testGettingAverage() GpuMat latestDepthMat = new GpuMat(); latestDepthMat.upload(cpuData); - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); Mat temp = new Mat(); currentAverage.download(temp); PerceptionDebugTools.printMat("current", temp, 1); From ab8ee1aa8f94f57bc017ffeecd75a40ad477aa31 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 12 Feb 2025 11:07:29 -0600 Subject: [PATCH 08/10] Alpha filter (ish) is working on the height map, UI button to turn the feature on and off was added as well. --- .../FilteredRapidHeightMapExtractor.java | 41 +++---- .../RapidHeightMapExtractorCUDA.java | 10 +- .../FilteredRapidHeightMapExtractor.cu | 30 +++-- .../FilteredRapidHeightMapExtractorTest.java | 107 +++++++++++++++++- .../heightMap/HeightMapParameters.java | 1 + .../heightMap/HeightMapParametersBasics.java | 5 + .../HeightMapParametersReadOnly.java | 5 + .../heightMap/HeightMapParameters.json | 1 + .../heightMap/HeightMapParametersGPU.json | 1 + 9 files changed, 159 insertions(+), 42 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 1af1ed7297b..34b3be4c08b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -20,9 +20,11 @@ public class FilteredRapidHeightMapExtractor { + static final int BLOCK_SIZE_XY = 32; + private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; - int layers = 2; + int layers = 10; private final CUstream_st stream; private final int rows; @@ -56,30 +58,13 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) CUDATools.checkCUDAError(error); currentIndex = 0; - - for (int i = 0; i < layers; i++) - { - // To get the average to be a whole number do (i * 2 + 2) - Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(0)); - - // Allocate memory for each layer, creating a 3d array - cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), - pointerTo3DArray.pitch(), - cpuData.data(), - cpuData.step(), - pointerTo3DArray.xsize(), - pointerTo3DArray.ysize(), - cudaMemcpyDefault); - } } public GpuMat update(GpuMat latestGlobalHeightMap, int defaultValue) { this.defaultValue = defaultValue; - GpuMat heightMapAverage = computerHeightMapHistoryAverage(latestGlobalHeightMap); - - return latestGlobalHeightMap; + return computerHeightMapHistoryAverage(latestGlobalHeightMap); } @NotNull @@ -105,20 +90,24 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) int error; GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - GpuMat variance = new GpuMat(rows, cols, opencv_core.CV_16UC1); kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); - kernel.withPointer(variance.data()).withLong(variance.step()); + kernel.withPointer(latestGlobalHeightMap.data()).withLong(latestGlobalHeightMap.step()); kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); kernel.withInt(defaultValue); - dim3 gridDim = new dim3(); - dim3 blockDim = new dim3(cols, rows, 1); - kernel.run(stream, gridDim, blockDim, 0); + //Execute the CUDA kernels with the provided stream + //Each kernel performs a specific task related to the height map update, registration, and cropping + int registerKernelGridSizeXY = (rows + BLOCK_SIZE_XY - 1) / BLOCK_SIZE_XY; + + dim3 blockSize = new dim3(BLOCK_SIZE_XY, BLOCK_SIZE_XY, 1); + dim3 registerKernelGridDim = new dim3(registerKernelGridSizeXY, registerKernelGridSizeXY, 1); + + kernel.run(stream, registerKernelGridDim, blockSize, 0); error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); @@ -127,7 +116,7 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) // In this example we go through 2,4,6,8 so we expect the average to be 5 Mat cpuResult = new Mat(); result.download(cpuResult); - PerceptionDebugTools.printMat("Result", cpuResult, 1); +// PerceptionDebugTools.printMat("Result", cpuResult, 1); cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), pointerTo3DArray.pitch(), @@ -147,7 +136,7 @@ public void reset() loopTracker = 0; } - public void close() + public void destroy() { program.close(); kernel.close(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index 2355840d2f0..fbdd329abd4 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -43,7 +43,7 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter private final GpuMat inputDepthImage; private final GpuMat localHeightMapImage; - private final GpuMat globalHeightMapImage; + private GpuMat globalHeightMapImage; private final GpuMat terrainCostImage; private final GpuMat contactMapImage; private final GpuMat sensorCroppedHeightMapImage; @@ -70,6 +70,7 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter private final FloatPointer worldToGroundTransformDevicePointer; private final FloatPointer parametersHostPointer; private final FloatPointer parametersDevicePointer; + private final FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor; public int sequenceNumber = 0; private float gridOffsetX; @@ -111,6 +112,8 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, terrainMapData = new TerrainMapData(heightMapParameters.getCropWindowSize(), heightMapParameters.getCropWindowSize()); recomputeDerivedParameters(); + // Need to initialize this after the parameters have been computed to get the right size + filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, globalCellsPerAxis, globalCellsPerAxis); try { @@ -206,6 +209,7 @@ public void reset() globalHeightMapImage.setTo(new Scalar(resetOffset)); emptyGlobalHeightMapImage.setTo(new Scalar(resetOffset)); + filteredRapidHeightMapExtractor.reset(); snappedFootstepsExtractor.reset(resetOffset); sequenceNumber = 0; @@ -296,6 +300,9 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); + if (heightMapParameters.getEnableAlphaFilter()) + globalHeightMapImage = filteredRapidHeightMapExtractor.update(globalHeightMapImage, 0); + // Run the cropping kernel croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step()); croppingKernel.withPointer(sensorCroppedHeightMapImage.data()).withLong(sensorCroppedHeightMapImage.step()); @@ -442,6 +449,7 @@ public void destroy() sensorCroppedHeightMapImage.close(); snappedFootstepsExtractor.destroy(); + filteredRapidHeightMapExtractor.destroy(); // At the end we have to destroy the stream to release the memory CUDAStreamManager.releaseStream(stream); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 1b8457ee913..f05a52e9ea2 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,4 +1,4 @@ -#define LAYERS 2 // Set a fixed size +#define LAYERS 10 // Set a fixed size extern "C" __global__ @@ -22,7 +22,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); +// printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } @@ -35,7 +35,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; float diff = (float)(*matrixPointerRow) - avg; - variance[layer] = diff * diff; + variance[layer] = abs(diff); } double heightSum = 0; @@ -47,26 +47,34 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - heightSum += (float)(*matrixPointerRow) / variance[layer]; - varianceSum += 1.0 / variance[layer]; + if (variance[layer] > 0.0f) // Prevent division by zero + { + heightSum += (double)(*matrixPointerRow) / (double)variance[layer]; + varianceSum += 1.0 / (double)variance[layer]; + } } +// printf("Height sum: %f\n", heightSum); +// printf("Variance sum: %f\n", varianceSum); unsigned short newHeight = heightSum / varianceSum; - unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; - float diff = (float) (*heightValue) - avg; - double newVariance = diff * diff; + + int heightValueInt = (int) *heightValue; + float diff = (float) heightValueInt - avg; + float newVariance = abs(diff); if (*heightValue == defaultValue) return; - unsigned short heightEstimate = (unsigned short) *heightValue + (newHeight * newVariance) / newVariance; + float alpha = 0.2; +// printf("Equation parameters (heightValue %hu) (alpha %f) (avg %hu)\n", *heightValue, alpha, avg); + float heightEstimate = (float) *heightValue * alpha + (avg * (1.0 - alpha)); // (newHeight * newVariance) / newVariance; unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; - *outputPointer = heightEstimate; + *outputPointer = (unsigned short) heightEstimate; - printf("GPU Height Estimate: %d\n", heightEstimate); +// printf("GPU Height Estimate: %f\n", heightEstimate); } diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java index c633eeaf563..859faa73d8d 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -14,8 +14,8 @@ public class FilteredRapidHeightMapExtractorTest @Test public void testGettingAverage() { - int rows = 2; - int cols = 2; + int rows = 1501; + int cols = 1501; CUstream_st stream = CUDAStreamManager.getStream(); FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); @@ -28,9 +28,108 @@ public void testGettingAverage() GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); Mat temp = new Mat(); currentAverage.download(temp); - PerceptionDebugTools.printMat("current", temp, 1); +// PerceptionDebugTools.printMat("current", temp, 1); + } + + filteredRapidHeightMapExtractor.destroy(); + } + + @Test + public void testChangeAfterSteadyState() + { + int rows = 2; + int cols = 2; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + // This example fills the history with the same value, all 8's + // Lets see how this behaves when we later then introduce a change (noise) + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + } + + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1000)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + + filteredRapidHeightMapExtractor.destroy(); + } + + @Test + public void testChangedAfterSteadyStateRealDepthValuesUnsignedShort() + { + int rows = 2; + int cols = 2; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + // This example fills the history with the same value, all 8's + // Lets see how this behaves when we later then introduce a change (noise) + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(32768)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + } + + // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(33100)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + + filteredRapidHeightMapExtractor.destroy(); + } + + + // TODO remove this guy heheeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee + @Test + public void testDefaultValueSkipping() + { + int rows = 3; + int cols = 3; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + // Set everything to 1, I'm gonna pass this in as the default value so these get skipped hopefully + for (int i = 0; i < 4; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + filteredRapidHeightMapExtractor.update(latestDepthMat, 0); } - filteredRapidHeightMapExtractor.close(); + //TODO maybe we can just skip this? Does it matter + // I guess the newest height map will have the same values, so we can skip it + + // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(33100)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + + filteredRapidHeightMapExtractor.destroy(); } } diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java index 9a7cf87aa3d..ba610fc5b27 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java @@ -19,6 +19,7 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP public static final StoredPropertyKeyList keys = new StoredPropertyKeyList(); public static final BooleanStoredPropertyKey resetHeightMap = keys.addBooleanKey("Reset Height Map"); + public static final BooleanStoredPropertyKey enableAlphaFilter = keys.addBooleanKey("Enable alpha filter"); public static final IntegerStoredPropertyKey searchWindowHeight = keys.addIntegerKey("Search window height"); public static final IntegerStoredPropertyKey searchWindowWidth = keys.addIntegerKey("Search window width"); public static final DoubleStoredPropertyKey minHeightRegistration = keys.addDoubleKey("Min height registration"); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java index a8aafa6e6f1..2588aaaa070 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java @@ -13,6 +13,11 @@ default void setResetHeightMap(boolean resetHeightMap) set(HeightMapParameters.resetHeightMap, resetHeightMap); } + default void setEnableAlphaFilter(boolean enableAlphaFilter) + { + set(HeightMapParameters.enableAlphaFilter, enableAlphaFilter); + } + default void setSearchWindowHeight(int searchWindowHeight) { set(HeightMapParameters.searchWindowHeight, searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java index 736cc22e610..1c73cff5731 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java @@ -15,6 +15,11 @@ default boolean getResetHeightMap() return get(resetHeightMap); } + default boolean getEnableAlphaFilter() + { + return get(enableAlphaFilter); + } + default int getSearchWindowHeight() { return get(searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json index e70622db6a2..4d038d2015c 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json @@ -1,6 +1,7 @@ { "title" : "HeightMapParameters", "Reset Height Map" : false, + "Enable alpha filter" : true, "Search window height": { "value": 20, "lowerBound": 10, diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index 56461a4ea28..33c41d8d2d4 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -1,6 +1,7 @@ { "title" : "GPU Height Map Parameters", "Reset Height Map" : false, + "Enable alpha filter" : true, "Search window height" : { "value" : 350, "lowerBound" : 10, From 8c0cd8eb61380f0022ec65a6ec4b111211a7e060 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 13 Feb 2025 08:14:25 -0600 Subject: [PATCH 09/10] Tuned filter a little bit --- .../gpuHeightMap/FilteredRapidHeightMapExtractor.java | 2 +- .../perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 34b3be4c08b..e55e1d2f72b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -24,7 +24,7 @@ public class FilteredRapidHeightMapExtractor private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; - int layers = 10; + int layers = 6; private final CUstream_st stream; private final int rows; diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index f05a52e9ea2..b1719835419 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,4 +1,4 @@ -#define LAYERS 10 // Set a fixed size +#define LAYERS 6 // Set a fixed size extern "C" __global__ From 582910a1508bd63a19333db08e03a8d1e9efce87 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sat, 15 Feb 2025 14:10:08 -0600 Subject: [PATCH 10/10] Update environments for the UI --- .../environments/CinderBlockPallet.json | 122 ++++----- .../environments/Simple Rough Terrain | 220 +++++++++++++++ .../environments/Uneven Terrain 3 Levels | 256 ++++++++++++++++++ .../environments/ZZZRoughTerrainOne.json | 174 ++++++------ 4 files changed, 624 insertions(+), 148 deletions(-) create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json b/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json index 52fcb505cb8..f1b6fe027bf 100644 --- a/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json @@ -102,56 +102,56 @@ }, { "type" : "RDXPalletObject", "x" : 2.4930577278137207, - "y" : 0.06989191472530365, - "z" : 0.1110124127939346, - "qx" : 0.00856238873690151, + "y" : 0.06989191472530099, + "z" : 0.08732389168723786, + "qx" : 0.008562388736901518, "qy" : -0.005114153010419687, - "qz" : 0.7018977765776746, - "qs" : 0.7122078644422724 + "qz" : 0.7018977765776755, + "qs" : 0.7122078644422707 }, { "type" : "RDXPalletObject", "x" : 3.513807535171509, - "y" : 0.04207326173782348, - "z" : 0.1237301762215778, - "qx" : 0.7094953522455109, - "qy" : 0.7046140939793347, - "qz" : 0.0030468615931168426, - "qs" : -0.011226768997909059 + "y" : 0.04207326173781967, + "z" : 0.08234225453700866, + "qx" : 0.7094654357241527, + "qy" : 0.7046729978293471, + "qz" : 0.007336564946010361, + "qs" : -0.006398160495962346 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.8034467697143555, - "y" : -0.26781320571899414, - "z" : 0.3262125167995693, - "qx" : -0.10285762375856672, - "qy" : 0.09713503866937312, - "qz" : 0.7006268231279272, - "qs" : 0.6993619579381686 + "x" : 2.803446769714356, + "y" : -0.26781320571898903, + "z" : 0.2791487080525257, + "qx" : -0.10285762375856766, + "qy" : 0.097135038669374, + "qz" : 0.7006268231279288, + "qs" : 0.6993619579381668 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.6078357696533203, - "y" : -0.2656903862953186, - "z" : 0.32625531796365964, - "qx" : -0.10647845066123686, - "qy" : 0.10638572963805844, - "qz" : 0.6993553654921224, - "qs" : 0.6987463694585758 + "x" : 2.61168138422286, + "y" : -0.2656870360892501, + "z" : 0.2783433061531242, + "qx" : -0.10647845066123718, + "qy" : 0.10638572963805866, + "qz" : 0.6993553654921232, + "qs" : 0.6987463694585748 }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 2.6733238697052, - "y" : 0.23532751202583313, - "z" : 0.30945813106372944, + "y" : 0.2353275120258309, + "z" : 0.23830242759757592, "qx" : 0.0, "qy" : 0.0, - "qz" : 0.2976377221605589, + "qz" : 0.29763772216055895, "qs" : 0.954678891746892 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.5680363178253174, - "y" : 0.41301260069012646, - "z" : 0.3042421745136381, - "qx" : -4.259043794782986E-5, - "qy" : 1.3876773247399921E-4, - "qz" : 0.2934102624957087, + "x" : 2.5662500681208926, + "y" : 0.389346157233353, + "z" : 0.2386235305666895, + "qx" : -4.259043794782999E-5, + "qy" : 1.387677324739995E-4, + "qz" : 0.2934102624957088, "qs" : 0.9559866091069321 }, { "type" : "RDXMediumCinderBlockRoughed", @@ -264,47 +264,47 @@ }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 3.216216802597046, - "y" : 0.4195704162120819, - "z" : 0.3607892571017146, - "qx" : 0.09720753880522989, - "qy" : 0.09797572562251415, - "qz" : -0.6975779785814231, - "qs" : 0.7030906167674457 + "y" : 0.41957041621207125, + "z" : 0.2886322178312223, + "qx" : 0.09720753880522992, + "qy" : 0.09797572562251418, + "qz" : -0.6975779785814232, + "qs" : 0.7030906167674456 }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 3.4069573879241943, - "y" : 0.41641926765441895, - "z" : 0.3601300859823824, - "qx" : 0.10611619192654026, - "qy" : 0.09930293387417374, - "qz" : -0.6917497248948593, - "qs" : 0.7073617173998588 + "y" : 0.41641926765440784, + "z" : 0.288369582091613, + "qx" : 0.10611619192654027, + "qy" : 0.09930293387417376, + "qz" : -0.6917497248948595, + "qs" : 0.7073617173998586 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.5369553565979004, - "y" : -0.34754684567451477, - "z" : 0.3093220075592398, - "qx" : 0.0, - "qy" : 0.0, - "qz" : -0.3582957156911485, - "qs" : 0.9336081512697754 + "x" : 3.5393702227098975, + "y" : -0.34539638462598843, + "z" : 0.22806068928635884, + "qx" : -0.0028667441390000164, + "qy" : 0.0019384903262589207, + "qz" : -0.3583054928379458, + "qs" : 0.9335979851282099 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.6725926399230957, - "y" : -0.20286306738853455, - "z" : 0.3029684722423553, + "x" : 3.672592639923095, + "y" : -0.20286306738852744, + "z" : 0.22763386235801902, "qx" : 0.0, "qy" : 0.0, - "qz" : -0.372481463577173, + "qz" : -0.37248146357717304, "qs" : 0.9280396323926081 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.8084611892700195, - "y" : -0.056925807148218155, - "z" : 0.30668363943696014, + "x" : 3.8095721431824052, + "y" : -0.06435738580480432, + "z" : 0.22950602759147598, "qx" : 0.0, "qy" : 0.0, - "qz" : -0.3759256660029729, + "qz" : -0.37592566600297295, "qs" : 0.9266498225544648 }, { "type" : "RDXPointLightObject", diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain new file mode 100644 index 00000000000..f89bb16c72f --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain @@ -0,0 +1,220 @@ +{ + "ambientLight" : 0.01, + "objects" : [ { + "type" : "RDXLabFloorObject", + "x" : 0.0, + "y" : 0.0, + "z" : 0.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0261326209306856, + "y" : -0.1312642229974892, + "z" : 0.04168685751514941, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0278165108023485, + "y" : 0.058344485126049284, + "z" : 0.041986173402213625, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0254283744607902, + "y" : 0.24169205928048654, + "z" : 0.04057212192335316, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0266319218745057, + "y" : -0.32255398445042305, + "z" : 0.041615879371108555, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0255996928843913, + "y" : 0.4299430060193219, + "z" : 0.039385070980956435, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.424579962380316, + "y" : 0.43113670704297236, + "z" : 0.09254610128793772, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.4259674753234353, + "y" : 0.24562213822075551, + "z" : 0.09061781008937408, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.4273566781817704, + "y" : 0.0589708940443876, + "z" : 0.08995922266082751, + "qx" : 0.003149536163718456, + "qy" : -1.0842021724855044E-19, + "qz" : 0.0, + "qs" : 0.9999950401986769 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.428751325767549, + "y" : -0.1308297033892502, + "z" : 0.08970254494993041, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.429117948670486, + "y" : -0.319230985173794, + "z" : 0.09054522513823815, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.889698199172627, + "y" : -0.31362771389739263, + "z" : 0.12029186241267142, + "qx" : -2.3889634179018873E-4, + "qy" : 0.09261829908698715, + "qz" : -1.3468733359722775E-6, + "qs" : 0.9957016589325124 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8925994110200004, + "y" : -0.12334371333174532, + "z" : 0.12113892318644881, + "qx" : -2.61922767217173E-4, + "qy" : 0.09538769007356435, + "qz" : 1.6653387936186146E-16, + "qs" : 0.9954401639369861 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.892382190045252, + "y" : 0.06702701050002413, + "z" : 0.1209962797071884, + "qx" : -4.073356737959721E-4, + "qy" : 0.09598398209739224, + "qz" : -1.6912157996040733E-5, + "qs" : 0.995382795196077 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8968050937558998, + "y" : 0.25091822068453606, + "z" : 0.1183742422242321, + "qx" : -7.310595024396378E-4, + "qy" : 0.09724279035586246, + "qz" : -5.628849670202829E-5, + "qs" : 0.995260419240821 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8967171650763754, + "y" : 0.4417683185753126, + "z" : 0.11887800146539662, + "qx" : -7.061977789265678E-4, + "qy" : 0.09361012578193359, + "qz" : -4.8221701554163054E-5, + "qs" : 0.9956086798087164 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.4854011650828687, + "y" : 0.337916486126029, + "z" : 0.030854774019734132, + "qx" : 0.07768412436974872, + "qy" : 0.07593874593882895, + "qz" : -0.7005910590819361, + "qs" : 0.7052451003875139 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.298407879878164, + "y" : 0.3385634336033867, + "z" : 0.030262727840291048, + "qx" : 0.07817291979346568, + "qy" : 0.07754413448983202, + "qz" : -0.7056224031799893, + "qs" : 0.6999806611240655 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.4801712617778033, + "y" : -0.17550711531681293, + "z" : 0.01956208485422517, + "qx" : -0.06847305311043973, + "qy" : 0.06771917439063006, + "qz" : 0.6945107854365579, + "qs" : 0.713007940579824 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2962901687151547, + "y" : -0.17636865286320602, + "z" : 0.022432163496095836, + "qx" : -0.06365197911790865, + "qy" : 0.06844083238574804, + "qz" : 0.6971263806041734, + "qs" : 0.7108298583222602 + } ] +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels new file mode 100644 index 00000000000..4745350e88f --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels @@ -0,0 +1,256 @@ +{ + "ambientLight" : 0.01, + "objects" : [ { + "type" : "RDXLabFloorObject", + "x" : 0.0, + "y" : 0.0, + "z" : 0.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPalletObject", + "x" : 3.0607671409344284, + "y" : -0.06310664378602587, + "z" : 0.07506210377440695, + "qx" : -1.0842021724855047E-19, + "qy" : 0.0028445913904392786, + "qz" : 6.93889390390723E-17, + "qs" : 0.9999959541417263 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.856897603762217, + "y" : -0.06462814957506413, + "z" : 0.04301444223836254, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.7725192909374095, + "y" : -0.3573444838618175, + "z" : 0.04315923678089523, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7033277782232468, + "qs" : 0.7108656950363771 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.9683418781865694, + "y" : -0.35782223569816507, + "z" : 0.04510366318073977, + "qx" : 0.0024008229853511027, + "qy" : -0.002327662684819567, + "qz" : -0.6960790454542465, + "qs" : 0.7179573667808722 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.7692433473042397, + "y" : 0.22736090391018615, + "z" : 0.04463407766635785, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.6991832031499666, + "qs" : 0.7149425490435946 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.958366039907915, + "y" : 0.22510670375295616, + "z" : 0.045219122127525546, + "qx" : 0.6955503250719798, + "qy" : 0.7184704667441876, + "qz" : -0.0029953814049817817, + "qs" : 9.80509505919548E-4 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.5778746753913864, + "y" : -0.35140658275500114, + "z" : 0.04094344602728395, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7108421321498666, + "qs" : 0.7033515928471561 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.5814870340806877, + "y" : 0.039696925710773545, + "z" : 0.04093598509427332, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7003398091610843, + "qs" : 0.7138096046595451 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.254567570271429, + "y" : -0.26246316058216784, + "z" : 0.08398364191738705, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2543383177500473, + "y" : -0.45188103127684, + "z" : 0.08437391630526325, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2567335113487488, + "y" : -0.07563787043052153, + "z" : 0.08331043805643118, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.256972731643839, + "y" : 0.1089200658313772, + "z" : 0.0831901145643251, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2568512020237517, + "y" : 0.2998872055301931, + "z" : 0.083989292309432, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6524621715840238, + "y" : 0.11525193387083688, + "z" : 0.2115532611096348, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6512512829296737, + "y" : 0.3088006898672824, + "z" : 0.2127774719692071, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6524316022722947, + "y" : -0.07200348851432764, + "z" : 0.2113775293633806, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6534353464148004, + "y" : -0.2624579488098271, + "z" : 0.20899677238844694, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6523451753964458, + "y" : -0.45492220028035296, + "z" : 0.20508528682018912, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.853173395299581, + "y" : -0.07850545679836562, + "z" : 0.046833812609385905, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.7660761367552613, + "y" : -0.3677813682161868, + "z" : 0.04590330560432699, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7068686309521961, + "qs" : 0.7073448512400214 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.9563662963951316, + "y" : -0.37261381461621124, + "z" : 0.04585163099189009, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.7079959342146718, + "qs" : 0.7062165086823545 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.761101071744873, + "y" : 0.2114722527067107, + "z" : 0.04291788963533616, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7036791662089575, + "qs" : 0.7105178611713194 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.953645008963975, + "y" : 0.21726026395490247, + "z" : 0.04430687538108835, + "qx" : -6.867242264466872E-4, + "qy" : 7.306747962616092E-4, + "qz" : -0.7066230485210534, + "qs" : 0.7075894726626394 + } ] +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json b/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json index fdb813bf62b..f89bb16c72f 100644 --- a/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json @@ -47,18 +47,18 @@ "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0261326209306865, - "y" : -0.13126422299748575, - "z" : 0.0, + "x" : 1.0261326209306856, + "y" : -0.1312642229974892, + "z" : 0.04168685751514941, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.027816510802349, - "y" : 0.0583444851260503, - "z" : 0.0, + "x" : 1.0278165108023485, + "y" : 0.058344485126049284, + "z" : 0.041986173402213625, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, @@ -66,71 +66,71 @@ }, { "type" : "RDXSmallCinderBlockRoughed", "x" : 1.0254283744607902, - "y" : 0.24169205928048704, - "z" : 1.1102230246251565E-16, + "y" : 0.24169205928048654, + "z" : 0.04057212192335316, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0266319218744941, - "y" : -0.32255398445039835, - "z" : 1.1102230246251565E-16, + "x" : 1.0266319218745057, + "y" : -0.32255398445042305, + "z" : 0.041615879371108555, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0255996928843942, - "y" : 0.4299430060193301, - "z" : 0.0, + "x" : 1.0255996928843913, + "y" : 0.4299430060193219, + "z" : 0.039385070980956435, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4245799623803117, - "y" : 0.4311367070429927, - "z" : 0.0, + "x" : 1.424579962380316, + "y" : 0.43113670704297236, + "z" : 0.09254610128793772, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4259674753234355, - "y" : 0.2456221382207675, - "z" : 0.0, + "x" : 1.4259674753234353, + "y" : 0.24562213822075551, + "z" : 0.09061781008937408, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4273566781817708, - "y" : 0.05897089404440137, - "z" : 0.0, - "qx" : 0.0, - "qy" : 0.0, + "x" : 1.4273566781817704, + "y" : 0.0589708940443876, + "z" : 0.08995922266082751, + "qx" : 0.003149536163718456, + "qy" : -1.0842021724855044E-19, "qz" : 0.0, - "qs" : 1.0 + "qs" : 0.9999950401986769 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.428751325767556, - "y" : -0.13082970338925246, - "z" : 0.0, + "x" : 1.428751325767549, + "y" : -0.1308297033892502, + "z" : 0.08970254494993041, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4291179486704777, - "y" : -0.3192309851737916, - "z" : 0.0, + "x" : 1.429117948670486, + "y" : -0.319230985173794, + "z" : 0.09054522513823815, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, @@ -138,83 +138,83 @@ }, { "type" : "RDXLargeCinderBlockRoughed", "x" : 1.889698199172627, - "y" : -0.3136277138974171, - "z" : 0.0, - "qx" : -2.3890013852761622E-4, - "qy" : 0.08700325143367432, - "qz" : -1.4571706576721966E-16, - "qs" : 0.9962079989473547 + "y" : -0.31362771389739263, + "z" : 0.12029186241267142, + "qx" : -2.3889634179018873E-4, + "qy" : 0.09261829908698715, + "qz" : -1.3468733359722775E-6, + "qs" : 0.9957016589325124 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.8925994110199995, - "y" : -0.12334371333177716, - "z" : 0.0, - "qx" : -2.619227672171765E-4, - "qy" : 0.09538769007356443, - "qz" : 2.2204403793185408E-16, + "x" : 1.8925994110200004, + "y" : -0.12334371333174532, + "z" : 0.12113892318644881, + "qx" : -2.61922767217173E-4, + "qy" : 0.09538769007356435, + "qz" : 1.6653387936186146E-16, "qs" : 0.9954401639369861 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.8923821900452733, - "y" : 0.06702701050002052, - "z" : 0.0, - "qx" : -4.074078878578608E-4, - "qy" : 0.09148835465765207, - "qz" : -1.5072662212893554E-5, - "qs" : 0.9958060628223065 + "x" : 1.892382190045252, + "y" : 0.06702701050002413, + "z" : 0.1209962797071884, + "qx" : -4.073356737959721E-4, + "qy" : 0.09598398209739224, + "qz" : -1.6912157996040733E-5, + "qs" : 0.995382795196077 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.896805093755894, - "y" : 0.25091822068453806, - "z" : 0.0, - "qx" : -7.314712274236847E-4, - "qy" : 0.08957687143892619, - "qz" : -5.0658014366053786E-5, - "qs" : 0.9959796416025897 + "x" : 1.8968050937558998, + "y" : 0.25091822068453606, + "z" : 0.1183742422242321, + "qx" : -7.310595024396378E-4, + "qy" : 0.09724279035586246, + "qz" : -5.628849670202829E-5, + "qs" : 0.995260419240821 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.896717165076377, - "y" : 0.4417683185753008, - "z" : 0.0, - "qx" : -7.061977789265809E-4, - "qy" : 0.09361012578193358, - "qz" : -4.822170155417867E-5, + "x" : 1.8967171650763754, + "y" : 0.4417683185753126, + "z" : 0.11887800146539662, + "qx" : -7.061977789265678E-4, + "qy" : 0.09361012578193359, + "qz" : -4.8221701554163054E-5, "qs" : 0.9956086798087164 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.4854011650828647, - "y" : 0.33791648612603054, - "z" : 0.0, + "x" : 2.4854011650828687, + "y" : 0.337916486126029, + "z" : 0.030854774019734132, "qx" : 0.07768412436974872, "qy" : 0.07593874593882895, "qz" : -0.7005910590819361, "qs" : 0.7052451003875139 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.2984078798781646, - "y" : 0.33856343360339525, - "z" : -0.004837357989063484, - "qx" : 0.08236230486456504, - "qy" : 0.08170002376624297, - "qz" : -0.7051456813991942, - "qs" : 0.6995077732649281 + "x" : 2.298407879878164, + "y" : 0.3385634336033867, + "z" : 0.030262727840291048, + "qx" : 0.07817291979346568, + "qy" : 0.07754413448983202, + "qz" : -0.7056224031799893, + "qs" : 0.6999806611240655 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.480171261777798, - "y" : -0.17550711531681523, - "z" : 0.0, - "qx" : -0.06847305311043997, - "qy" : 0.06771917439063073, - "qz" : 0.6945107854365578, + "x" : 2.4801712617778033, + "y" : -0.17550711531681293, + "z" : 0.01956208485422517, + "qx" : -0.06847305311043973, + "qy" : 0.06771917439063006, + "qz" : 0.6945107854365579, "qs" : 0.713007940579824 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.296290168715155, - "y" : -0.1763686528631838, - "z" : 0.0, - "qx" : -0.0710345669393736, - "qy" : 0.07596834185010112, - "qz" : 0.6964128483016595, - "qs" : 0.7100648182079236 + "x" : 2.2962901687151547, + "y" : -0.17636865286320602, + "z" : 0.022432163496095836, + "qx" : -0.06365197911790865, + "qy" : 0.06844083238574804, + "qz" : 0.6971263806041734, + "qs" : 0.7108298583222602 } ] } \ No newline at end of file