added desired version of cpu p2l

src/rmagine_core/include/rmagine/math/statistics.h

@@ -44,6 +44,7 @@
 #include "math.h"
 #include <rmagine/types/shared_functions.h>
 #include "linalg.h"
+#include <unordered_map>
 
 
 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_<RAM>& 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<CrossStatistics>& 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_<RAM>& dataset,
+    const PointCloudView_<RAM>& model,
+    const MemoryView<Transform>& model_pretransforms,
+    const UmeyamaReductionConstraints params,
+    MemoryView<CrossStatistics>& 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<unsigned int, CrossStatistics> 
+ */
+// std::unordered_map<unsigned int, CrossStatistics> statistics_p2l_ow(
+//     const PointCloudView_<RAM>& dataset,
+//     const Transform& pre_transform,
+//     const PointCloudView_<RAM>& model,
+//     const UmeyamaReductionConstraints params);
+
+
 } // namespace rmagine
 
 #endif // RMAGINE_MATH_STATISTICS_H

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_<RAM>& dataset,
+    const PointCloudView_<RAM>& model,
+    const UmeyamaReductionConstraints params,
+    MemoryView<CrossStatistics>& stats)
+{
+    CrossStatistics st = CrossStatistics::Identity();
+
+    // #pragma omp parallel for reduction(+: st)
+    for(size_t i=0; i<dataset.points.size(); i++)
+    {
+        if(    (dataset.mask.empty() || dataset.mask[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_<RAM>& dataset,
+    const PointCloudView_<RAM>& model,
+    const MemoryView<Transform>& model_pretransforms, // we would need more pre_transforms here
+    const UmeyamaReductionConstraints params,
+    MemoryView<CrossStatistics>& 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<n_objects; i++)
+    {
+        stats[i] = CrossStatistics::Identity();
+    }
+
+    for(size_t i=0; i<n_measurements; i++)
+    {
+        if(    (dataset.mask.empty() || dataset.mask[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<unsigned int, CrossStatistics> statistics_p2l_ow(
+//     const Transform& pre_transform,
+//     const PointCloudView_<RAM>& dataset,
+//     const PointCloudView_<RAM>& model,
+//     const UmeyamaReductionConstraints params)
+// {
+//     std::unordered_map<unsigned int, CrossStatistics> ret;
+
+//     // TODO
+//     throw std::runtime_error("TODO");
+
+//     return ret;
+// }
+
+
 } // namespace rmagine 

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());
     }