added working correction test using optix ray casting correspondences…

… and reduction on GPU
uos · Nov 12, 2024 · 8f0f6be · 8f0f6be
1 parent d6cf301
commit 8f0f6be
Show file tree

Hide file tree

Showing 6 changed files with 227 additions and 63 deletions.
diff --git a/src/rmagine_core/include/rmagine/types/PointCloud.hpp b/src/rmagine_core/include/rmagine/types/PointCloud.hpp
@@ -51,7 +51,7 @@ template<typename MemT>
 struct PointCloud_
 {
     Memory<Vector, MemT>        points;
-    Memory<unsigned int, MemT>  mask;
+    Memory<uint8_t, MemT>       mask;
     Memory<Vector, MemT>        normals;
     Memory<unsigned int, MemT>  ids;
 };
@@ -62,9 +62,9 @@ template<typename MemT>
 struct PointCloudView_
 {
     MemoryView<Vector, MemT>        points; // required
-    MemoryView<unsigned int, MemT>  mask    = MemoryView<unsigned int, MemT>::Empty();
+    MemoryView<uint8_t, MemT>       mask    = MemoryView<uint8_t, MemT>::Empty();
     MemoryView<Vector, MemT>        normals = MemoryView<Vector, MemT>::Empty();
-    MemoryView<unsigned int, MemT>  ids     = MemoryView<unsigned int, MemT>::Empty();
+    MemoryView<unsigned int, MemT>  ids     = MemoryView<uint32_t, MemT>::Empty();
 };
 
 // default: RAM

diff --git a/src/rmagine_cuda/src/math/statistics.cu b/src/rmagine_cuda/src/math/statistics.cu
@@ -58,11 +58,11 @@ __global__ void sum_kernel(
 template<unsigned int nMemElems>
 __global__ void statistics_p2p_kernel(
     const Vector*   dataset_points,
-    const unsigned int* dataset_mask,
+    const uint8_t* dataset_mask,
     const unsigned int* dataset_ids,
     const Transform pre_transform,
     const Vector*   model_points,
-    const unsigned int* model_mask,
+    const uint8_t* model_mask,
     const unsigned int* model_ids,
     const UmeyamaReductionConstraints params,
     unsigned int N,
@@ -147,7 +147,7 @@ void statistics_p2p(
     MemoryView<CrossStatistics, VRAM_CUDA>& stats)
 {
     const unsigned int n_outputs = stats.size(); // also number of blocks
-    constexpr unsigned int n_threads = 1024; // also shared mem
+    constexpr unsigned int n_threads = 512; // also shared mem
 
     statistics_p2p_kernel<n_threads> <<<n_outputs, n_threads>>>(
         dataset.points.raw(), dataset.mask.raw(), dataset.ids.raw(), 
@@ -187,16 +187,15 @@ CrossStatistics statistics_p2p(
     return ret;
 }
 
-
 template<unsigned int nMemElems>
 __global__ void statistics_p2l_kernel(
     const Vector*   dataset_points,
-    const unsigned int* dataset_mask,
+    const uint8_t* dataset_mask,
     const unsigned int* dataset_ids,
     const Transform pre_transform,
     const Vector*   model_points,
     const Vector*   model_normals,
-    const unsigned int* model_mask,
+    const uint8_t* model_mask,
     const unsigned int* model_ids,
     const UmeyamaReductionConstraints params,
     unsigned int N,
@@ -285,7 +284,7 @@ void statistics_p2l(
     MemoryView<CrossStatistics, VRAM_CUDA>& stats)
 {
     const unsigned int n_outputs = stats.size(); // also number of blocks
-    constexpr unsigned int n_threads = 1024; // also shared mem
+    constexpr unsigned int n_threads = 512; // also shared mem
 
     statistics_p2l_kernel<n_threads> <<<n_outputs, n_threads>>>(
         dataset.points.raw(), dataset.mask.raw(), dataset.ids.raw(), 

diff --git a/tests/core/math_statistics.cpp b/tests/core/math_statistics.cpp
@@ -446,7 +446,7 @@ void test_p2p()
 
     rm::Memory<rm::Vector3> dataset_points(n_points);
     rm::Memory<rm::Vector3> model_points(n_points);
-    rm::Memory<unsigned int> dataset_mask(n_points);
+    rm::Memory<uint8_t> dataset_mask(n_points);
     rm::Memory<unsigned int> dataset_ids(n_points);
 
     // rm::Memory<unsigned int> mask;
@@ -539,7 +539,7 @@ void test_p2l()
     rm::Memory<rm::Vector3> dataset_points(n_points);
     rm::Memory<rm::Vector3> model_points(n_points);
     rm::Memory<rm::Vector3> model_normals(n_points);
-    rm::Memory<unsigned int> dataset_mask(n_points);
+    rm::Memory<uint8_t> dataset_mask(n_points);
     rm::Memory<unsigned int> dataset_ids(n_points);
 
     // rm::Memory<unsigned int> mask;

diff --git a/tests/embree/correction_rcc.cpp b/tests/embree/correction_rcc.cpp
@@ -1,8 +1,12 @@
 #include <iostream>
+#include <memory>
+#include <cassert>
+#include <sstream>
+
 #include <rmagine/map/EmbreeMap.hpp>
 #include <rmagine/map/embree/embree_shapes.h>
 #include <rmagine/map/embree/EmbreeScene.hpp>
-#include <memory>
+
 
 #include <rmagine/simulation/SphereSimulatorEmbree.hpp>
 #include <rmagine/types/sensors.h>
@@ -12,21 +16,20 @@
 
 #include <rmagine/util/prints.h>
 #include <rmagine/util/exceptions.h>
-#include <cassert>
-#include <sstream>
+
 
 
 namespace rm = rmagine;
 
-template<typename DataT>
-void printStats(rm::CrossStatistics_<DataT> stats)
-{
-    std::cout << "CrossStatistics: " << std::endl;
-    std::cout << "- dataset mean: " << stats.dataset_mean << std::endl;
-    std::cout << "- model mean: " << stats.model_mean << std::endl;
-    std::cout << "- cov: " << stats.covariance << std::endl;
-    std::cout << "- n meas: " << stats.n_meas << std::endl; 
-}
+// template<typename DataT>
+// void printStats(rm::CrossStatistics_<DataT> stats)
+// {
+//     std::cout << "CrossStatistics: " << std::endl;
+//     std::cout << "- dataset mean: " << stats.dataset_mean << std::endl;
+//     std::cout << "- model mean: " << stats.model_mean << std::endl;
+//     std::cout << "- cov: " << stats.covariance << std::endl;
+//     std::cout << "- n meas: " << stats.n_meas << std::endl; 
+// }
 
 rm::EmbreeMapPtr make_map()
 {
@@ -50,7 +53,7 @@ void cast(rm::MemoryView<Tfrom, rm::RAM> from, rm::MemoryView<Tto, rm::RAM> to)
     }
 }
 
-unsigned int count(rm::MemoryView<unsigned int, rm::RAM> data)
+unsigned int count(rm::MemoryView<uint8_t, rm::RAM> data)
 {
     unsigned int ret = 0;
     for(size_t i=0; i<data.size(); i++)
@@ -60,16 +63,16 @@ unsigned int count(rm::MemoryView<unsigned int, rm::RAM> data)
     return ret;
 }
 
-void printCorrespondences(
-    const rm::PointCloudView& cloud_dataset,
-    const rm::PointCloudView& cloud_model)
-{
-    std::cout << cloud_dataset.points.size() << " to " << cloud_model.points.size() << std::endl;
-    for(size_t i=0; i<cloud_dataset.points.size(); i++)
-    {
-        std::cout << cloud_dataset.points[i] << " -> " << cloud_model.points[i] << std::endl;
-    }
-}
+// void printCorrespondences(
+//     const rm::PointCloudView& cloud_dataset,
+//     const rm::PointCloudView& cloud_model)
+// {
+//     std::cout << cloud_dataset.points.size() << " to " << cloud_model.points.size() << std::endl;
+//     for(size_t i=0; i<cloud_dataset.points.size(); i++)
+//     {
+//         std::cout << cloud_dataset.points[i] << " -> " << cloud_model.points[i] << std::endl;
+//     }
+// }
 
 rm::SphericalModel define_sensor_model()
 {
@@ -89,7 +92,7 @@ rm::SphericalModel define_sensor_model()
 
 int main(int argc, char** argv)
 {
-    std::cout << "Correction RCC" << std::endl;
+    std::cout << "Correction Embree-RCC + CPU optimization" << std::endl;
 
     // create data
     rm::SphereSimulatorEmbree sim;
@@ -108,63 +111,47 @@ int main(int argc, char** argv)
     // define what we want to simulate and pre-malloc all buffers
     rm::IntAttrAll<rm::RAM> dataset;
     rm::resize_memory_bundle<rm::RAM>(dataset, sensor_model.getWidth(), sensor_model.getHeight(), 1);
-
-    rm::Memory<unsigned int, rm::RAM> dataset_mask(sensor_model.size());
 
     sim.setTsb(rm::Transform::Identity());
     sim.simulate(Tbm, dataset);
 
-    cast(dataset.hits, dataset_mask);
-
     rm::PointCloudView cloud_dataset = {
         .points = dataset.points,
-        .mask = dataset_mask
+        .mask = dataset.hits
     };
 
-    { // SOME CHECKS ON THE DATA
-        unsigned int tmp = count(dataset_mask);
-        std::cout << tmp << " hits" << std::endl;
+    assert(count(dataset.hits) > 0);
 
-        if(tmp == 0)
-        {
-            for(size_t i=0; i<dataset.ranges.size(); i++)
-            {
-                std::cout << dataset.ranges[i] << " ";
-            }
-            std::cout << std::endl;
-        }
-
-        assert(tmp > 0);
-    }
 
+    /////////////////////////
+    // MICP params
     // correspondence searches
     size_t n_outer = 5;
-
     // optimization steps using the same correspondences
     size_t n_inner = 5;
+    rm::UmeyamaReductionConstraints params;
+    params.max_dist = 100.0;
+    ///////////////////////////
 
     // pose of robot
     rm::Transform Tbm_est = rm::Transform::Identity();
     Tbm_est.t.z = 0.1; // perturbe the pose
-
 
     std::cout << "0: " << Tbm_est << " -> " << Tbm_gt << std::endl;
 
     // pre-create buffers for RCC
     rm::IntAttrAll<rm::RAM> model;
     rm::resize_memory_bundle<rm::RAM>(model, sensor_model.getWidth(), sensor_model.getHeight(), 1);
-    rm::Memory<unsigned int, rm::RAM> model_mask(sensor_model.size());
 
     for(size_t i=0; i<n_outer; i++)
     {
         // find RCC at estimated pose of robot
         rm::MemoryView<rm::Transform> Tbm_est_view(&Tbm_est, 1);
         sim.simulate(Tbm_est_view, model);
-        cast(model.hits, model_mask);
 
         rm::PointCloudView cloud_model = {
             .points = model.points,
-            .mask = model_mask,
+            .mask = model.hits,
             .normals = model.normals
         };
 
@@ -174,8 +161,6 @@ int main(int argc, char** argv)
         rm::Transform Tpre = rm::Transform::Identity();
         for(size_t j=0; j<n_inner; j++)
         {
-            rm::UmeyamaReductionConstraints params;
-            params.max_dist = 100.0;
             rm::CrossStatistics stats = rm::statistics_p2l(Tpre, cloud_dataset, cloud_model, params);
             rm::Transform Tpre_next = rm::umeyama_transform(stats);
             Tpre = Tpre * Tpre_next;

diff --git a/tests/optix/CMakeLists.txt b/tests/optix/CMakeLists.txt
@@ -33,4 +33,12 @@ target_link_libraries(rmagine_tests_optix_simulation_ondn
     rmagine::optix
 )
 
-add_test(NAME optix_simulation_ondn COMMAND rmagine_tests_optix_simulation_ondn)
+add_test(NAME optix_simulation_ondn COMMAND rmagine_tests_optix_simulation_ondn)
+
+# 5. Correction RCC
+add_executable(rmagine_tests_optix_correction_rcc correction_rcc.cpp)
+target_link_libraries(rmagine_tests_optix_correction_rcc
+    rmagine::optix
+)
+
+add_test(NAME optix_correction_rcc COMMAND rmagine_tests_optix_correction_rcc)