Merge branch 'develop'

apps/carmen2simplemap/carmen2simplemap_main.cpp

@@ -27,7 +27,7 @@
 #include <mrpt/maps/CSimpleMap.h>
 #include <mrpt/obs/CObservationOdometry.h>
 #include <mrpt/obs/carmen_log_tools.h>
-#include <mrpt/poses/CPosePDFGaussian.h>
+#include <mrpt/poses/CPose3DPDFGaussian.h>
 #include <mrpt/serialization/CArchive.h>
 #include <mrpt/system/datetime.h>
 #include <mrpt/system/filesystem.h>
@@ -153,9 +153,8 @@ int main(int argc, char** argv)
 				}
 
 				// Insert (observations, pose) pair:
-				CPosePDFGaussian::Ptr pos =
-					std::make_shared<CPosePDFGaussian>();
-				pos->mean = gt_pose;
+				auto pos = CPose3DPDFGaussian::Create();
+				pos->mean = mrpt::poses::CPose3D(gt_pose);
 				theSimpleMap.insert(pos, SF);
 			}
 

apps/map-partition/map-partition.cpp

@@ -100,14 +100,11 @@ void Test()
 	const size_t n = in_map.size();
 	for (size_t i = 0; i < n; i++)
 	{
-		CSensoryFrame::Ptr sf;
-		CPose3DPDF::Ptr posePDF;
-
-		in_map.get(i, posePDF, sf);
+		const auto& [posePDF, sf, twist] = in_map.get(i);
 
 		imp.addMapFrame(*sf, *posePDF);
 
-		printf("[%u/%u]...", (unsigned int)i, (unsigned int)n);
+		printf("[%zu/%zu]...", i, n);
 
 		//		if ((i%1)==0)
 		if (i == n - 1)
@@ -154,11 +151,7 @@ void Test()
 		out_map.clear();
 		for (unsigned int j : parts[i])
 		{
-			CSensoryFrame::Ptr sf;
-			CPose3DPDF::Ptr posePDF;
-
-			in_map.get(j, posePDF, sf);
-
+			const auto& [posePDF, sf, twist] = in_map.get(j);
 			out_map.insert(posePDF, sf);
 		}
 
@@ -262,11 +255,8 @@ void Test()
 		CPose2D meanPose;
 		for (size_t j = 0; j < parts[i].size(); j++)
 		{
-			CSensoryFrame::Ptr sf;
-			CPose3DPDF::Ptr posePDF;
-
 			// Get the pose:
-			in_map.get(parts[i][j], posePDF, sf);
+			const auto& [posePDF, sf, twist] = in_map.get(parts[i][j]);
 
 			meanPose = CPose2D(posePDF->getMeanVal());
 

apps/observations2map/observations2map_main.cpp

@@ -92,7 +92,7 @@ int main(int argc, char** argv)
 		// Build metric maps:
 		cout << "Building metric maps..." << std::endl;
 
-		metricMap.loadFromProbabilisticPosesAndObservations(simplemap);
+		metricMap.loadFromSimpleMap(simplemap);
 
 		cout << "done." << endl;
 

apps/robot-map-gui/CDocument.cpp

@@ -158,7 +158,7 @@ std::vector<size_t> CDocument::remove(const std::vector<size_t>& indexes)
 
 void CDocument::move(
 	const std::vector<size_t>& indexes,
-	const CSimpleMap::TPosePDFSensFramePairList& posesObsPairs)
+	const CSimpleMap::KeyframeList& posesObsPairs)
 {
 	for (size_t i = 0; i < indexes.size(); ++i)
 		move(indexes[i], posesObsPairs[i], true);
@@ -168,29 +168,28 @@ void CDocument::move(
 }
 
 void CDocument::move(
-	size_t index, const CSimpleMap::Pair& posesObsPair,
+	size_t index, const CSimpleMap::Keyframe& posesObsPair,
 	bool disableUpdateMetricMap)
 {
-	m_simplemap.set(index, posesObsPair);
+	auto& kf = m_simplemap.get(index);
+	kf = posesObsPair;
 
 	m_changedFile = true;
 	if (!disableUpdateMetricMap) updateMetricMap();
 }
 
 void CDocument::insert(
-	const std::vector<size_t>& idx,
-	CSimpleMap::TPosePDFSensFramePairList& posesObsPairs)
+	const std::vector<size_t>& idx, CSimpleMap::KeyframeList& posesObsPairs)
 {
 	for (size_t i = 0; i < idx.size(); ++i)
 		m_simplemap.insert(posesObsPairs[i]);
 
 	updateMetricMap();
 }
 
-CSimpleMap::TPosePDFSensFramePairList CDocument::get(
-	const std::vector<size_t>& idxs)
+CSimpleMap::KeyframeList CDocument::get(const std::vector<size_t>& idxs)
 {
-	CSimpleMap::TPosePDFSensFramePairList posesObsPairs;
+	CSimpleMap::KeyframeList posesObsPairs;
 	for (auto& idx : idxs)
 	{
 		auto pair = get(idx);
@@ -199,21 +198,11 @@ CSimpleMap::TPosePDFSensFramePairList CDocument::get(
 	return posesObsPairs;
 }
 
-CSimpleMap::ConstPair CDocument::get(size_t idx) const
+CSimpleMap::KeyframeList CDocument::getReverse(const std::vector<size_t>& idx)
 {
-	return m_simplemap.getAsPair(idx);
-}
-mrpt::maps::CSimpleMap::Pair CDocument::get(size_t idx)
-{
-	return m_simplemap.getAsPair(idx);
-}
-
-CSimpleMap::TPosePDFSensFramePairList CDocument::getReverse(
-	const std::vector<size_t>& idx)
-{
-	CSimpleMap::TPosePDFSensFramePairList posesObsPairs;
+	CSimpleMap::KeyframeList posesObsPairs;
 	for (int i = idx.size() - 1; i >= 0; --i)
-		posesObsPairs.emplace_back(m_simplemap.getAsPair(idx[i]));
+		posesObsPairs.emplace_back(m_simplemap.get(idx[i]));
 
 	return posesObsPairs;
 }
@@ -236,7 +225,7 @@ void CDocument::addMapToRenderizableMaps(
 
 void CDocument::updateMetricMap()
 {
-	m_metricmap.loadFromProbabilisticPosesAndObservations(m_simplemap);
+	m_metricmap.loadFromSimpleMap(m_simplemap);
 
 	m_typeConfigs.clear();
 	m_typeConfigs.emplace(TypeOfConfig::PointsMap, std::vector<MetricPtr>());

apps/robot-map-gui/CDocument.h

@@ -64,22 +64,28 @@ class CDocument
 
 	void move(
 		const std::vector<size_t>& indexes,
-		const mrpt::maps::CSimpleMap::TPosePDFSensFramePairList& posesObsPairs);
+		const mrpt::maps::CSimpleMap::KeyframeList& posesObsPairs);
 	void move(
-		size_t index, const mrpt::maps::CSimpleMap::Pair& posesObsPair,
+		size_t index, const mrpt::maps::CSimpleMap::Keyframe& posesObsPair,
 		bool disableUpdateMetricMap = false);
 
 	void insert(
 		const std::vector<size_t>& idx,
-		mrpt::maps::CSimpleMap::TPosePDFSensFramePairList& posesObsPairs);
+		mrpt::maps::CSimpleMap::KeyframeList& posesObsPairs);
 
-	mrpt::maps::CSimpleMap::TPosePDFSensFramePairList get(
-		const std::vector<size_t>& idx);
+	mrpt::maps::CSimpleMap::KeyframeList get(const std::vector<size_t>& idx);
+
+	const mrpt::maps::CSimpleMap::Keyframe& get(size_t idx) const
+	{
+		return m_simplemap.get(idx);
+	}
 
-	mrpt::maps::CSimpleMap::ConstPair get(size_t idx) const;
-	mrpt::maps::CSimpleMap::Pair get(size_t idx);
+	mrpt::maps::CSimpleMap::Keyframe& get(size_t idx)
+	{
+		return m_simplemap.get(idx);
+	}
 
-	mrpt::maps::CSimpleMap::TPosePDFSensFramePairList getReverse(
+	mrpt::maps::CSimpleMap::KeyframeList getReverse(
 		const std::vector<size_t>& idx);
 
    private:

apps/robot-map-gui/gui/CMainWindow.cpp

@@ -339,8 +339,7 @@ void CMainWindow::selectedChanged(const std::vector<size_t>& idx)
 }
 
 void CMainWindow::addRobotPosesFromMap(
-	std::vector<size_t> idx,
-	mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs)
+	std::vector<size_t> idx, mrpt::maps::CSimpleMap::KeyframeList posesObsPairs)
 {
 	if (!m_document || idx.empty()) return;
 
@@ -363,8 +362,7 @@ void CMainWindow::moveRobotPosesOnMap(
 {
 	if (!m_document || idx.empty()) return;
 
-	mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs =
-		m_document->get(idx);
+	mrpt::maps::CSimpleMap::KeyframeList posesObsPairs = m_document->get(idx);
 	for (auto& poseSf : posesObsPairs)
 	{
 		mrpt::poses::CPose3DPDF::Ptr& posePDF = poseSf.pose;
@@ -398,7 +396,7 @@ void CMainWindow::deleteRobotPoses(const std::vector<size_t>& idx)
 {
 	if (!m_document || idx.empty()) return;
 
-	mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs =
+	mrpt::maps::CSimpleMap::KeyframeList posesObsPairs =
 		m_document->getReverse(idx);
 
 	auto reverseInd = m_document->remove(idx);
@@ -469,7 +467,7 @@ void CMainWindow::updateDirection(
 {
 	if (!m_document) return;
 
-	mrpt::maps::CSimpleMap::Pair posesObsPair = m_document->get(index);
+	auto posesObsPair = m_document->get(index);
 
 	auto posePDF = posesObsPair.pose;
 	auto pose = posePDF->getMeanVal();

apps/robot-map-gui/gui/CMainWindow.h

@@ -36,7 +36,7 @@ class CMainWindow : public QMainWindow
 
 	void addRobotPosesFromMap(
 		std::vector<size_t> idx,
-		mrpt::maps::CSimpleMap::TPosePDFSensFramePairList posesObsPairs);
+		mrpt::maps::CSimpleMap::KeyframeList posesObsPairs);
 	void deleteRobotPosesFromMap(const std::vector<size_t>& idx);
 	void moveRobotPosesOnMap(
 		const std::vector<size_t>& idx, const QPointF& dist);

apps/robot-map-gui/gui/observationTree/CPairNode.cpp

@@ -16,7 +16,7 @@ using namespace mrpt;
 using namespace mrpt::maps;
 
 CPairNode::CPairNode(
-	CNode* parent, const CSimpleMap::Pair& poseSensFramePair,
+	CNode* parent, const CSimpleMap::Keyframe& poseSensFramePair,
 	size_t indexInSimpleMap)
 	: CNode(parent), m_indexInSimpleMap(indexInSimpleMap)
 {

apps/robot-map-gui/gui/observationTree/CPairNode.h

@@ -20,7 +20,8 @@ class CPairNode : public CNode
 {
    public:
 	CPairNode(
-		CNode* parent, const mrpt::maps::CSimpleMap::Pair& poseSensFramePair,
+		CNode* parent,
+		const mrpt::maps::CSimpleMap::Keyframe& poseSensFramePair,
 		size_t indexInSimpleMap);
 	~CPairNode() override;
 

appveyor.yml

@@ -1,5 +1,5 @@
 # version format
-version: 2.11.4-{branch}-build{build}
+version: 2.11.5-{branch}-build{build}
 
 os: Visual Studio 2019
 

doc/source/doxygen-docs/changelog.md

@@ -1,5 +1,18 @@
 \page changelog Change Log
 
+# Version 2.11.5: Released Dec 21st, 2023
+- Changes in libraries:
+  - \ref mrpt_maps_grp
+    - New method mrpt::maps::CPointsMap::insertPointFrom() (and associated auxiliary methods) to easily copy points between different point clouds with different fields (timestamp, ring, RGB, etc.).
+  - \ref mrpt_obs_grp
+    - mrpt::maps::CSimpleMap changes:
+      - Added an optional twist field.
+      - Simplified API for preferred usage with structured binding tuples.
+  - \ref mrpt_system_grp
+    - More readable results in mrpt::system::unitsFormat() for the special case of exactly `0`.
+- BUG FIXES:
+  - Fix filtering of NANs input point clouds in mrpt::maps::CPointsMap::insertAnotherMap().
+
 # Version 2.11.4: Released Dec 15th, 2023
 - Changes in apps:
   - RawLogViewer: visualize mrpt::obs::CObservationRotatingScan as point cloud  + range image + intensity image.

libs/apps/src/CGridMapAlignerApp.cpp

@@ -27,7 +27,7 @@
 #include <mrpt/opengl/CSetOfLines.h>
 #include <mrpt/opengl/Scene.h>
 #include <mrpt/poses/CPoint2D.h>
-#include <mrpt/poses/CPosePDFGaussian.h>
+#include <mrpt/poses/CPose3DPDFGaussian.h>
 #include <mrpt/poses/CPosePDFParticles.h>
 #include <mrpt/random.h>
 #include <mrpt/serialization/CArchive.h>
@@ -348,7 +348,7 @@ void CGridMapAlignerApp::run()
 	}
 
 	// Generate the_map1 now:
-	the_map1.loadFromProbabilisticPosesAndObservations(map1);
+	the_map1.loadFromSimpleMap(map1);
 
 	size_t N1 = max(
 		40,
@@ -389,10 +389,7 @@ void CGridMapAlignerApp::run()
 
 			for (unsigned int q = 0; q < map2noisy.size(); q++)
 			{
-				CPose3DPDF::Ptr PDF;
-				CSensoryFrame::Ptr SF;
-
-				map2noisy.get(q, PDF, SF);
+				const auto [PDF, SF, twist] = map2noisy.get(q);
 
 				// If it's detect_test, translate the map2 by a fixed, known
 				// quantity:
@@ -421,26 +418,28 @@ void CGridMapAlignerApp::run()
 
 				if (NOISE_IN_POSE)
 				{
-					CPosePDFGaussian::Ptr newPDF =
-						std::make_shared<CPosePDFGaussian>();
+					auto& kf = map2noisy.get(q);
+					auto newPDF = std::make_shared<CPose3DPDFGaussian>();
 					newPDF->copyFrom(*PDF);
 
 					// Change the pose:
 					newPDF->mean.x_incr(
 						getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY));
 					newPDF->mean.y_incr(
 						getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY));
-					newPDF->mean.phi_incr(getRandomGenerator().drawGaussian1D(
-						0, DEG2RAD(STD_NOISE_PHI)));
-					newPDF->mean.normalizePhi();
 
-					// Change into the map:
-					map2noisy.set(q, newPDF, CSensoryFrame::Ptr());
+					double yaw, pitch, roll;
+					newPDF->mean.getYawPitchRoll(yaw, pitch, roll);
+					yaw += getRandomGenerator().drawGaussian1D(
+						0, DEG2RAD(STD_NOISE_PHI));
+					newPDF->mean.setYawPitchRoll(yaw, pitch, roll);
 
-				}  // end of NOISE_IN_POSE
+					// Change into the map:
+					kf.pose = newPDF;
+				}
 			}
 
-			the_map2.loadFromProbabilisticPosesAndObservations(map2noisy);
+			the_map2.loadFromSimpleMap(map2noisy);
 
 			size_t N2 = max(
 				40,

libs/apps/src/MonteCarloLocalization_App.cpp

@@ -299,7 +299,7 @@ void MonteCarloLocalization_Base::do_pf_localization()
 			// Build metric map:
 			// ------------------------------
 			MRPT_LOG_INFO("Building metric map(s) from '.simplemap'...");
-			metricMap->loadFromProbabilisticPosesAndObservations(simpleMap);
+			metricMap->loadFromSimpleMap(simpleMap);
 			MRPT_LOG_INFO("Done.");
 		}
 		else if (!mapExt.compare("gridmap"))

libs/maps/include/mrpt/maps/CColouredPointsMap.h

@@ -37,7 +37,7 @@ class CColouredPointsMap : public CPointsMap
 	CColouredPointsMap(const CPointsMap& o) { CPointsMap::operator=(o); }
 	CColouredPointsMap(const CColouredPointsMap& o) : CPointsMap()
 	{
-		impl_copyFrom(o);
+		CColouredPointsMap::impl_copyFrom(o);
 	}
 	CColouredPointsMap& operator=(const CPointsMap& o)
 	{
@@ -227,6 +227,20 @@ class CColouredPointsMap : public CPointsMap
 	 * predefined value */
 	void resetPointsMinDist(float defValue = 2000.0f);
 
+	// clang-format off
+	auto getPointsBufferRef_color_R() const    -> const mrpt::aligned_std_vector<float>* override { return &m_color_R;}
+	auto getPointsBufferRef_color_G() const    -> const mrpt::aligned_std_vector<float>* override { return &m_color_G; }
+	auto getPointsBufferRef_color_B() const    -> const mrpt::aligned_std_vector<float>* override { return &m_color_B; }
+
+	auto getPointsBufferRef_color_R()          -> mrpt::aligned_std_vector<float>* override { return &m_color_R; }
+	auto getPointsBufferRef_color_G()          -> mrpt::aligned_std_vector<float>* override { return &m_color_G; }
+	auto getPointsBufferRef_color_B()          -> mrpt::aligned_std_vector<float>* override { return &m_color_B; }
+
+	void insertPointField_color_R(float v) override { m_color_R.push_back(v); }
+	void insertPointField_color_G(float v) override { m_color_G.push_back(v); }
+	void insertPointField_color_B(float v) override { m_color_B.push_back(v); }
+	/// clang-format on
+
 	/** @name PCL library support
 		@{ */