diff --git a/src/rmagine_core/include/rmagine/math/statistics.h b/src/rmagine_core/include/rmagine/math/statistics.h index 74c55db..75ae592 100644 --- a/src/rmagine_core/include/rmagine/math/statistics.h +++ b/src/rmagine_core/include/rmagine/math/statistics.h @@ -44,6 +44,7 @@ #include "math.h" #include #include "linalg.h" +#include namespace rmagine @@ -74,6 +75,8 @@ struct UmeyamaReductionConstraints * - set set id view of dataset or model to enable filtering by ID, which can be set via 'UmeyamaReductionConstraints'. * - set max_dist of UmeyamaReductionConstraints for a maximum allowed distance * + * See: "MICP-L: Mesh-based ICP for Robot Localization using Hardware-Accelerated Ray Casting" + * * @param[in] pre_transform use this transformation to pretransform the dataset. set to Transform::Identity() for no transform. * @param[in] dataset PointCloudView pointing to buffers of the dataset * @param[in] model PointCloudView pointing to buffers of the model @@ -110,6 +113,8 @@ CrossStatistics statistics_p2p( * - set set id view of dataset or model to enable filtering by ID, which can be set via 'UmeyamaReductionConstraints'. * - set max_dist of UmeyamaReductionConstraints for a maximum allowed distance * + * See: "MICP-L: Mesh-based ICP for Robot Localization using Hardware-Accelerated Ray Casting" + * * @param[in] pre_transform use this transformation to pretransform the dataset. set to Transform::Identity() for no transform. * @param[in] dataset PointCloudView pointing to buffers of the dataset * @param[in] model PointCloudView pointing to buffers of the model @@ -136,6 +141,56 @@ CrossStatistics statistics_p2l( const PointCloudView_& model, const UmeyamaReductionConstraints params); + +/** + * @brief Find out cross statistics between dataset and model, where the model consists of several objects (intances or geometries) + * + * Given an unsegmented scan and an initial guess of the scene, we can compute a cross statistic between the scan and the model's objects. + * It returns one cross statistic per object id. Which can be used to deform the scene in a computationally efficient manner. + * See: "Mesh-based Object Tracking for Dynamic Semantic 3D Scene Graphs via Ray Tracing" + * + * This version requires a pre-allocated view of the correct size. For a more dynamic variant use the statistics_p2l_ow with the map return. + * + * MemoryView& stats: + * it is most efficient for the object ids to be dense in order to not waste memory + * e.g model_object_ids could look like this: + * [-,-,-,0,2,-,-,-,1,-,-,-,-,3] -> requires 3 memory + * but not like this + * [-,-,-,1000,-,1] -> requires 1000 memory + * + * @param pre_transform + * @param dataset + * @param model + * @param params + * @param[out] stats A list of stats + */ +void statistics_p2l_ow( + const PointCloudView_& dataset, + const PointCloudView_& model, + const MemoryView& model_pretransforms, + const UmeyamaReductionConstraints params, + MemoryView& stats); + +/** + * @brief Find out cross statistics between dataset and model, where the model consists of several objects (intances or geometries) + * + * Given an unsegmented scan and an initial guess of the scene, we can compute a cross statistic between the scan and the model's objects. + * It returns one cross statistic per object id. Which can be used to deform the scene in a computationally efficient manner. + * See: "Mesh-based Object Tracking for Dynamic Semantic 3D Scene Graphs via Ray Tracing" + * + * @param pre_transform + * @param dataset + * @param model + * @param params + * @return std::unordered_map + */ +// std::unordered_map statistics_p2l_ow( +// const PointCloudView_& dataset, +// const Transform& pre_transform, +// const PointCloudView_& model, +// const UmeyamaReductionConstraints params); + + } // namespace rmagine #endif // RMAGINE_MATH_STATISTICS_H \ No newline at end of file diff --git a/src/rmagine_core/src/math/statistics.cpp b/src/rmagine_core/src/math/statistics.cpp index 4228824..3912506 100644 --- a/src/rmagine_core/src/math/statistics.cpp +++ b/src/rmagine_core/src/math/statistics.cpp @@ -221,6 +221,48 @@ void statistics_p2l( stats[0].n_meas = n_meas; } + +RMAGINE_HOST_FUNCTION +void statistics_p2l_iwantthis_butnans( + const Transform& pre_transform, + const PointCloudView_& dataset, + const PointCloudView_& model, + const UmeyamaReductionConstraints params, + MemoryView& stats) +{ + CrossStatistics st = CrossStatistics::Identity(); + + // #pragma omp parallel for reduction(+: st) + for(size_t i=0; i 0) + && (model.mask.empty() || model.mask[i] > 0) + && (dataset.ids.empty() || dataset.ids[i] == params.dataset_id) + && (model.ids.empty() || model.ids[i] == params.model_id) + ) + { + const Vector Di = pre_transform * dataset.points[i]; // read + const Vector Ii = model.points[i]; // read + const Vector Ni = model.normals[i]; + + // 2. project new dataset point on plane -> new model point + const float signed_plane_dist = (Ii - Di).dot(Ni); + + if(fabs(signed_plane_dist) < params.max_dist) + { + // nearest point on model + const Vector Mi = Di + Ni * signed_plane_dist; + + // add Di -> Mi correspondence + st += CrossStatistics::Init(Di, Mi); + } + } + } + + // write + stats[0] = st; +} + RMAGINE_HOST_FUNCTION void statistics_p2l( const Transform& pre_transform, @@ -246,4 +288,65 @@ CrossStatistics statistics_p2l( return ret; } +RMAGINE_HOST_FUNCTION +void statistics_p2l_ow( + const PointCloudView_& dataset, + const PointCloudView_& model, + const MemoryView& model_pretransforms, // we would need more pre_transforms here + const UmeyamaReductionConstraints params, + MemoryView& stats) +{ + // TODO: test this function! + unsigned int n_measurements = dataset.points.size(); + unsigned int n_objects = stats.size(); + + // initialize all partitions + for(size_t i=0; i 0) + && (model.mask.empty() || model.mask[i] > 0) + ) + { + const unsigned int Oi = model.ids[i]; + const Transform Tmodel = model_pretransforms[Oi]; + + // 1. read a few things we need to make a iterative + const Vector Di = dataset.points[i]; + const Vector Ii = Tmodel * model.points[i]; + const Vector Ni = Tmodel.R * model.normals[i]; + + // 2. project new dataset point on plane -> new model point + const float signed_plane_dist = (Ii - Di).dot(Ni); + if(fabs(signed_plane_dist) < params.max_dist) + { + // nearest point on model + const Vector Mi = Di + Ni * signed_plane_dist; + // Or Mi -> Di here? since our model is going to matched to our dataset - reversed to localization + stats[Oi] += CrossStatistics::Init(Di, Mi); + } + } + } +} + +// RMAGINE_HOST_FUNCTION +// std::unordered_map statistics_p2l_ow( +// const Transform& pre_transform, +// const PointCloudView_& dataset, +// const PointCloudView_& model, +// const UmeyamaReductionConstraints params) +// { +// std::unordered_map ret; + +// // TODO +// throw std::runtime_error("TODO"); + +// return ret; +// } + + } // namespace rmagine \ No newline at end of file diff --git a/tests/optix/correction_rcc.cpp b/tests/optix/correction_rcc.cpp index 028ac52..0d71a7f 100644 --- a/tests/optix/correction_rcc.cpp +++ b/tests/optix/correction_rcc.cpp @@ -147,7 +147,6 @@ int main(int argc, char** argv) rm::CrossStatistics stats = rm::statistics_p2l(Tpre, cloud_dataset, cloud_model, params); // printStats(stats); - rm::Transform Tpre_next = rm::umeyama_transform(stats); Tpre = Tpre * Tpre_next; } @@ -164,7 +163,7 @@ int main(int argc, char** argv) if(fabs(Tdiff.t.z) > 0.001) { std::stringstream ss; - ss << "Embree Correction RCC results wrong!"; + ss << "OptiX Correction RCC results wrong!"; RM_THROW(rm::EmbreeException, ss.str()); }