Import new insert point with context

Author: jlblancoc

modules/mrpt_maps/include/mrpt/maps/CPointsMap.h

@@ -176,9 +176,16 @@ class CPointsMap :
     m_z[index] = z;
   }
 
-  /** The virtual method for \a insertPoint() *without* calling
-   * mark_as_modified()   */
-  virtual void insertPointFast(float x, float y, float z = 0) = 0;
+  /** Low-level method for \a insertPoint() *without* calling  mark_as_modified().
+   * Note that for derived classes having more per-point fields, you must ensure adding those fields
+   * after this to keep the length of all vectors consistent.
+   */
+  void insertPointFast(float x, float y, float z)
+  {
+    m_x.push_back(x);
+    m_y.push_back(y);
+    m_z.push_back(z);
+  }
 
   /** Get all the data fields for one point as a vector: depending on the
    * implementation class this can be [X Y Z] or [X Y Z R G B], etc...
@@ -205,9 +212,10 @@ class CPointsMap :
     this->clear();
     this->reserve(N);
     this->registerPointFieldsFrom(obj);
+    const auto ctx = this->prepareForInsertPointsFrom(obj);
     for (size_t i = 0; i < N; i++)
     {
-      insertPointFrom(obj, i);
+      insertPointFrom(obj, i, ctx);
     }
   }
 
@@ -803,28 +811,56 @@ class CPointsMap :
     return allAdded;
   }
 
+  /** Insert context for insertPointFrom().
+   * \sa prepareForInsertPointsFrom()
+   */
+  struct InsertCtx
+  {
+    // For XYZ
+    const mrpt::aligned_std_vector<float>* xs_src = nullptr;
+    const mrpt::aligned_std_vector<float>* ys_src = nullptr;
+    const mrpt::aligned_std_vector<float>* zs_src = nullptr;
+
+    // Optional field mappings (only fields present in both maps)
+    struct FloatFieldMapping
+    {
+      const mrpt::aligned_std_vector<float>* src_buf = nullptr;
+      mrpt::aligned_std_vector<float>* dst_buf = nullptr;
+    };
+    std::vector<FloatFieldMapping> float_fields;
+
+    struct UInt16FieldMapping
+    {
+      const mrpt::aligned_std_vector<uint16_t>* src_buf = nullptr;
+      mrpt::aligned_std_vector<uint16_t>* dst_buf = nullptr;
+    };
+    std::vector<UInt16FieldMapping> uint16_fields;
+  };
+
+  /** Prepare efficient data structures for repeated insertion from another point map with
+   * insertPointFrom()  */
+  InsertCtx prepareForInsertPointsFrom(const CPointsMap& source) const;
+
   /** Generic method to copy *all* applicable point properties from
    *  one map to another, e.g. timestamp, intensity, etc.
    *  \note Before calling this in a loop, make sure of calling registerPointFieldsFrom()
    */
-  void insertPointFrom(const mrpt::maps::CPointsMap& source, size_t sourcePointIndex)
+  void insertPointFrom(const CPointsMap& source, size_t sourcePointIndex, const InsertCtx& ctx)
   {
-    const auto i = sourcePointIndex;  // shortcut
-    // mandatory fields: XYZ
-    const auto& xs = source.getPointsBufferRef_x();
-    const auto& ys = source.getPointsBufferRef_y();
-    const auto& zs = source.getPointsBufferRef_z();
-    insertPointFast(xs[i], ys[i], zs[i]);
-
-    // Optional fields: only if they already exist on both maps:
-    for (const auto& f : this->getPointFieldNames_float_except_xyz())
+    const size_t i = sourcePointIndex;
+    insertPointFast((*ctx.xs_src)[i], (*ctx.ys_src)[i], (*ctx.zs_src)[i]);
+
+    // Optional fields
+    for (auto& f : ctx.float_fields)
     {
-      insertPointField_float(f, source.getPointField_float(i, f));
+      f.dst_buf->push_back((*f.src_buf)[i]);
     }
-    for (const auto& f : this->getPointFieldNames_uint16())
+
+    for (auto& f : ctx.uint16_fields)
     {
-      insertPointField_uint16(f, source.getPointField_uint16(i, f));
+      f.dst_buf->push_back((*f.src_buf)[i]);
     }
+
     mark_as_modified();
   }
 
@@ -1176,7 +1212,9 @@ class CPointsMap :
     clear();
     reserve(N);
     for (size_t i = 0; i < N; ++i)
+    {
       this->insertPointFast(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
+    }
   }
 
   /** @} */

modules/mrpt_maps/src/maps/CPointsMap.cpp

@@ -1528,6 +1528,8 @@ void CPointsMap::insertAnotherMap(
   // matrix multiplications:
   const bool identity_tf = (otherPose == CPose3D::Identity());
 
+  const auto ctx = this->prepareForInsertPointsFrom(*otherMap);
+
   mrpt::math::TPoint3Df pt;
   for (size_t srcIdx = 0; srcIdx < N_other; srcIdx++)
   {
@@ -1542,7 +1544,7 @@ void CPointsMap::insertAnotherMap(
     if (pt.x != pt.x) continue;
 
     // Add to this map:
-    this->insertPointFrom(*otherMap, srcIdx);
+    this->insertPointFrom(*otherMap, srcIdx, ctx);
 
     // and overwrite the XYZ, if needed:
     if (!identity_tf)
@@ -2056,6 +2058,48 @@ std::vector<std::string_view> CPointsMap::getPointFieldNames_float_except_xyz()
   return result;
 }
 
+CPointsMap::InsertCtx CPointsMap::prepareForInsertPointsFrom(const CPointsMap& source) const
+{
+  InsertCtx ctx;
+
+  // Mandatory XYZ
+  ctx.xs_src = &source.getPointsBufferRef_x();
+  ctx.ys_src = &source.getPointsBufferRef_y();
+  ctx.zs_src = &source.getPointsBufferRef_z();
+
+  // Optional fields: match by name but store direct pointers
+  const auto src_float_names = source.getPointFieldNames_float_except_xyz();
+  const auto dst_float_names = this->getPointFieldNames_float_except_xyz();
+
+  for (const auto& name : dst_float_names)
+  {
+    if (auto it = std::find(src_float_names.begin(), src_float_names.end(), name);
+        it != src_float_names.end())
+    {
+      ctx.float_fields.push_back(
+          {source.getPointsBufferRef_float_field(name),
+           const_cast<mrpt::aligned_std_vector<float>*>(
+               this->getPointsBufferRef_float_field(name))});
+    }
+  }
+
+  const auto src_u16_names = source.getPointFieldNames_uint16();
+  const auto dst_u16_names = this->getPointFieldNames_uint16();
+  for (const auto& name : dst_u16_names)
+  {
+    if (auto it = std::find(src_u16_names.begin(), src_u16_names.end(), name);
+        it != src_u16_names.end())
+    {
+      ctx.uint16_fields.push_back(
+          {source.getPointsBufferRef_uint_field(name),
+           const_cast<mrpt::aligned_std_vector<uint16_t>*>(
+               this->getPointsBufferRef_uint_field(name))});
+    }
+  }
+
+  return ctx;
+}
+
 // =========== API of the NearestNeighborsCapable virtual interface ======
 // See docs in base class
 void CPointsMap::nn_prepare_for_2d_queries() const