From 8f0f6be7e8ac0cb2e9bb1554873bcd2784451aca Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Tue, 12 Nov 2024 16:41:10 +0100 Subject: [PATCH] added working correction test using optix ray casting correspondences and reduction on GPU --- .../include/rmagine/types/PointCloud.hpp | 6 +- src/rmagine_cuda/src/math/statistics.cu | 13 +- tests/core/math_statistics.cpp | 4 +- tests/embree/correction_rcc.cpp | 85 ++++----- tests/optix/CMakeLists.txt | 10 +- tests/optix/correction_rcc.cpp | 172 ++++++++++++++++++ 6 files changed, 227 insertions(+), 63 deletions(-) create mode 100644 tests/optix/correction_rcc.cpp diff --git a/src/rmagine_core/include/rmagine/types/PointCloud.hpp b/src/rmagine_core/include/rmagine/types/PointCloud.hpp index c2ae716..1f2bde0 100644 --- a/src/rmagine_core/include/rmagine/types/PointCloud.hpp +++ b/src/rmagine_core/include/rmagine/types/PointCloud.hpp @@ -51,7 +51,7 @@ template struct PointCloud_ { Memory points; - Memory mask; + Memory mask; Memory normals; Memory ids; }; @@ -62,9 +62,9 @@ template struct PointCloudView_ { MemoryView points; // required - MemoryView mask = MemoryView::Empty(); + MemoryView mask = MemoryView::Empty(); MemoryView normals = MemoryView::Empty(); - MemoryView ids = MemoryView::Empty(); + MemoryView ids = MemoryView::Empty(); }; // default: RAM diff --git a/src/rmagine_cuda/src/math/statistics.cu b/src/rmagine_cuda/src/math/statistics.cu index 154aa22..c82ff98 100644 --- 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 __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& 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 <<>>( dataset.points.raw(), dataset.mask.raw(), dataset.ids.raw(), @@ -187,16 +187,15 @@ CrossStatistics statistics_p2p( return ret; } - template __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& 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 <<>>( dataset.points.raw(), dataset.mask.raw(), dataset.ids.raw(), diff --git a/tests/core/math_statistics.cpp b/tests/core/math_statistics.cpp index f8675d4..7e3db99 100644 --- a/tests/core/math_statistics.cpp +++ b/tests/core/math_statistics.cpp @@ -446,7 +446,7 @@ void test_p2p() rm::Memory dataset_points(n_points); rm::Memory model_points(n_points); - rm::Memory dataset_mask(n_points); + rm::Memory dataset_mask(n_points); rm::Memory dataset_ids(n_points); // rm::Memory mask; @@ -539,7 +539,7 @@ void test_p2l() rm::Memory dataset_points(n_points); rm::Memory model_points(n_points); rm::Memory model_normals(n_points); - rm::Memory dataset_mask(n_points); + rm::Memory dataset_mask(n_points); rm::Memory dataset_ids(n_points); // rm::Memory mask; diff --git a/tests/embree/correction_rcc.cpp b/tests/embree/correction_rcc.cpp index c00485a..a8215d0 100644 --- a/tests/embree/correction_rcc.cpp +++ b/tests/embree/correction_rcc.cpp @@ -1,8 +1,12 @@ #include +#include +#include +#include + #include #include #include -#include + #include #include @@ -12,21 +16,20 @@ #include #include -#include -#include + namespace rm = rmagine; -template -void printStats(rm::CrossStatistics_ 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 +// void printStats(rm::CrossStatistics_ 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 from, rm::MemoryView to) } } -unsigned int count(rm::MemoryView data) +unsigned int count(rm::MemoryView data) { unsigned int ret = 0; for(size_t i=0; i 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_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_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 dataset; rm::resize_memory_bundle(dataset, sensor_model.getWidth(), sensor_model.getHeight(), 1); - - rm::Memory 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 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 model; rm::resize_memory_bundle(model, sensor_model.getWidth(), sensor_model.getHeight(), 1); - rm::Memory model_mask(sensor_model.size()); for(size_t i=0; i 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 +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +namespace rm = rmagine; + +template +void printStats(rm::CrossStatistics_ 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::OptixMapPtr make_map() +{ + rm::OptixScenePtr scene = std::make_shared(); + + rm::OptixGeometryPtr mesh = std::make_shared(); + // mesh->apply(); + mesh->commit(); + scene->add(mesh); + scene->commit(); + + return std::make_shared(scene); +} + +rm::SphericalModel define_sensor_model() +{ + rm::SphericalModel model; + model.theta.min = -M_PI; + model.theta.inc = 1.0 * DEG_TO_RAD_F; + model.theta.size = 360; + + model.phi.min = -64.0 * DEG_TO_RAD_F; + model.phi.inc = 4.0 * DEG_TO_RAD_F; + model.phi.size = 32; + + model.range.min = 0.5; + model.range.max = 130.0; + return model; +} + +template +void cast(rm::MemoryView from, rm::MemoryView to) +{ + for(size_t i=0; i(from[i]); + } +} + +unsigned int count(rm::MemoryView data) +{ + unsigned int ret = 0; + for(size_t i=0; i Tbm(1); + Tbm[0] = Tbm_gt; + + // define what we want to simulate and pre-malloc all buffers + rm::IntAttrAll dataset; + rm::resize_memory_bundle(dataset, sensor_model.getWidth(), sensor_model.getHeight(), 1); + + sim.setTsb(rm::Transform::Identity()); + sim.simulate(Tbm, dataset); + + rm::PointCloudView_ cloud_dataset = { + .points = dataset.points, + .mask = dataset.hits + }; + + //////////////////////////// + // 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 model; + rm::resize_memory_bundle(model, sensor_model.getWidth(), sensor_model.getHeight(), 1); + + for(size_t i=0; i Tbm_est_view(&Tbm_est, 1); + sim.simulate(Tbm_est_view, model); + + rm::PointCloudView_ cloud_model = { + .points = model.points, + .mask = model.hits, + .normals = model.normals + }; + + // this describes a transformation in time of the base frame + // source: base, t+1 (after registration) + // target: base, t (before registration) + rm::Transform Tpre = rm::Transform::Identity(); + for(size_t j=0; j (base,t) before transforming from (base,t) -> (map) + Tbm_est = Tbm_est * Tpre; + std::cout << i+1 << ": " << Tbm_est << " -> " << Tbm_gt << std::endl; + } + + // diff from one base frame to the other + // transform from gt to estimation base frame + auto Tdiff = ~Tbm_est * Tbm_gt; + if(fabs(Tdiff.t.z) > 0.001) + { + std::stringstream ss; + ss << "Embree Correction RCC results wrong!"; + RM_THROW(rm::EmbreeException, ss.str()); + } + + return 0; +} \ No newline at end of file