WIP: points API

MRPT · Dec 19, 2023 · 0420072 · 0420072
1 parent e6bcbcb
commit 0420072
Show file tree

Hide file tree

Showing 3 changed files with 88 additions and 13 deletions.
diff --git a/libs/maps/include/mrpt/maps/CPointsMap.h b/libs/maps/include/mrpt/maps/CPointsMap.h
@@ -575,20 +575,21 @@ class CPointsMap : public CMetricMap,
 	{
 		return m_z;
 	}
-
-	virtual auto getPointsBufferRef_intensity() const
+	// clang-format off
+	virtual auto getPointsBufferRef_intensity() const  -> const mrpt::aligned_std_vector<float>* { return nullptr; }
+	virtual auto getPointsBufferRef_ring() const       -> const mrpt::aligned_std_vector<uint16_t>* { return nullptr; }
+	virtual auto getPointsBufferRef_timestamp() const  -> const mrpt::aligned_std_vector<float>* { return nullptr; }
+	virtual auto getPointsBufferRef_color_R() const
 		-> const mrpt::aligned_std_vector<float>*
 	{
 		return nullptr;
 	}
-
-	virtual auto getPointsBufferRef_ring() const
-		-> const mrpt::aligned_std_vector<uint16_t>*
+	virtual auto getPointsBufferRef_color_G() const
+		-> const mrpt::aligned_std_vector<float>*
 	{
 		return nullptr;
 	}
-
-	virtual auto getPointsBufferRef_timestamp() const
+	virtual auto getPointsBufferRef_color_B() const
 		-> const mrpt::aligned_std_vector<float>*
 	{
 		return nullptr;
@@ -599,19 +600,57 @@ class CPointsMap : public CMetricMap,
 	{
 		return nullptr;
 	}
-
 	virtual auto getPointsBufferRef_ring()
 		-> mrpt::aligned_std_vector<uint16_t>*
 	{
 		return nullptr;
 	}
-
 	virtual auto getPointsBufferRef_timestamp()
 		-> mrpt::aligned_std_vector<float>*
 	{
 		return nullptr;
 	}
 
+	virtual auto getPointsBufferRef_color_R()
+		-> mrpt::aligned_std_vector<float>*
+	{
+		return nullptr;
+	}
+	virtual auto getPointsBufferRef_color_G()
+		-> mrpt::aligned_std_vector<float>*
+	{
+		return nullptr;
+	}
+	virtual auto getPointsBufferRef_color_B()
+		-> mrpt::aligned_std_vector<float>*
+	{
+		return nullptr;
+	}
+
+	bool hasField_Intensity() const
+	{
+		return getPointsBufferRef_intensity() != nullptr;
+	}
+	bool hasField_Ring() const { return getPointsBufferRef_ring() != nullptr; }
+	bool hasField_Timestamp() const
+	{
+		return getPointsBufferRef_timestamp() != nullptr;
+	}
+
+	virtual void insertPointField_Intensity([[maybe_unused]] float i)
+	{
+		// default: none
+	}
+	virtual void insertPointField_Ring([[maybe_unused]] uint16_t r)
+	{
+		// default: none
+	}
+	virtual void insertPointField_Timestamp([[maybe_unused]] float t)
+	{
+		// default: none
+	}
+	/// clang-format on
+
 	/** Returns a copy of the 2D/3D points as a std::vector of float
 	 * coordinates.
 	 * If decimation is greater than 1, only 1 point out of that number will be
@@ -708,6 +747,29 @@ class CPointsMap : public CMetricMap,
 		insertPoint(x, y, z);
 	}
 
+	/** Generic method to copy *all* applicable point properties from
+	 *  one map to another, e.g. timestamp, intensity, etc.
+	 */
+	void insertPointFrom(
+		const mrpt::maps::CPointsMap& source, size_t sourcePointIndex)
+	{
+		const auto i = sourcePointIndex;  // shortcut
+		const auto& xs = source.getPointsBufferRef_x();
+		const auto& ys = source.getPointsBufferRef_y();
+		const auto& zs = source.getPointsBufferRef_z();
+		const auto* Is = source.getPointsBufferRef_intensity();
+		const auto* Rs = source.getPointsBufferRef_ring();
+		const auto* Ts = source.getPointsBufferRef_timestamp();
+
+		// XYZ:
+		insertPointFast(xs[i], ys[i], z[i]);
+		if (Is && hasField_intensity()) insertPointField_Intensity((*Is)[i]);
+		if (Rs && hasField_ring()) insertPointField_Ring((*Rs)[i]);
+		if (Ts && hasField_Timestamp()) insertPointField_Timestamp((*Ts)[i]);
+
+		mark_as_modified();
+	}
+
 	/** Set all the points at once from vectors with X,Y and Z coordinates (if Z
 	 * is not provided, it will be set to all zeros).
 	 * \tparam VECTOR can be mrpt::math::CVectorFloat or std::vector<float> or

diff --git a/libs/maps/src/maps/CPointsMapXYZI.cpp b/libs/maps/src/maps/CPointsMapXYZI.cpp
@@ -107,8 +107,11 @@ void CPointsMapXYZI::impl_copyFrom(const CPointsMap& obj)
 	// This also does a ::resize(N) of all data fields.
 	CPointsMap::base_copyFrom(obj);
 
-	const auto* pXYZI = dynamic_cast<const CPointsMapXYZI*>(&obj);
-	if (pXYZI) m_intensity = pXYZI->m_intensity;
+	ASSERT_EQUAL_(m_x.size(), m_intensity.size());
+
+	if (const auto* Is = obj.getPointsBufferRef_intensity(); Is)
+		m_intensity = *Is;
+	// else: leave with default values in this class
 }
 
 uint8_t CPointsMapXYZI::serializeGetVersion() const { return 0; }

diff --git a/libs/maps/src/maps/CPointsMapXYZIRT.cpp b/libs/maps/src/maps/CPointsMapXYZIRT.cpp
@@ -159,8 +159,18 @@ void CPointsMapXYZIRT::impl_copyFrom(const CPointsMap& obj)
 	// This also does a ::resize(N) of all data fields.
 	CPointsMap::base_copyFrom(obj);
 
-	const auto* pXYZI = dynamic_cast<const CPointsMapXYZIRT*>(&obj);
-	if (pXYZI) m_intensity = pXYZI->m_intensity;
+	if (const auto* Is = obj.getPointsBufferRef_intensity(); Is)
+		m_intensity = *Is;
+	else
+		m_intensity.clear();
+
+	if (const auto* Rs = obj.getPointsBufferRef_ring(); Rs) m_ring = *Rs;
+	else
+		m_ring.clear();
+
+	if (const auto* Ts = obj.getPointsBufferRef_timestamp(); Ts) m_time = *Ts;
+	else
+		m_time.clear();
 }
 
 uint8_t CPointsMapXYZIRT::serializeGetVersion() const { return 0; }