pymrpt: better obs_to_viz() example

doc/source/doxygen-docs/changelog.md

@@ -8,6 +8,7 @@
     - mrpt::comms::CInterfaceFTDI Fix usage of deprecated API in libftdi
   - \ref mrpt_obs_grp
     - New static method mrpt::obs::CRawlog::ReadFromArchive() (useful for python bindings)
+    - New overload mrpt::obs::obs_to_viz() for mrpt::obs::CSensoryFrame containers
 - Python:
   - New wrapped functions:
     - `mrpt.serialization.archiveFrom()`

doc/source/pymrpt_example_global_localization.rst

@@ -9,6 +9,11 @@ algorithm for a robot from a dataset in .rawlog format.
 The adaptive particle filter algorithm has two implementations, 
 for 2D and 3D poses, or SE(2) or SE(3), respectively.
 
+.. raw:: html
+
+    <iframe width="560" height="315" src="https://www.youtube.com/embed/ZfVUydQIM5E" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen>
+    </iframe>
+
 
 .. literalinclude:: ../../python-examples/global_localization.py
    :language: python

libs/maps/include/mrpt/obs/customizable_obs_viz.h

@@ -72,6 +72,21 @@ bool obs_to_viz(
 	const mrpt::obs::CObservation::Ptr& obs, const VisualizationParameters& p,
 	mrpt::opengl::CSetOfObjects& out);
 
+/** Clears `out` and creates a visualization of the given sensory-frame,
+ *  dispatching the call according to the actual observation classes inside the
+ * SF.
+ *
+ *  \return true if type has known visualizer, false if it does not (then, `out`
+ *          will be empty)
+ *
+ *  \note This and the accompanying functions are defined in namespace
+ *        mrpt::obs, but you must link against mrpt::maps too to have their
+ *        definitions.
+ */
+bool obs_to_viz(
+	const mrpt::obs::CSensoryFrame& sf, const VisualizationParameters& p,
+	mrpt::opengl::CSetOfObjects& out);
+
 /// Clears `out` and creates a visualization of the given observation.
 void obs3Dscan_to_viz(
 	const mrpt::obs::CObservation3DRangeScan::Ptr& obs,

libs/maps/src/obs/customizable_obs_viz.cpp

@@ -10,6 +10,7 @@
 #include "maps-precomp.h"  // Precomp header
 //
 #include <mrpt/maps/CColouredPointsMap.h>
+#include <mrpt/obs/CSensoryFrame.h>
 #include <mrpt/obs/customizable_obs_viz.h>
 #include <mrpt/opengl/CAxis.h>
 #include <mrpt/opengl/CPlanarLaserScan.h>
@@ -331,3 +332,28 @@ bool mrpt::obs::obs_to_viz(
 	out.clear();
 	return false;
 }
+
+bool mrpt::obs::obs_to_viz(
+	const mrpt::obs::CSensoryFrame& sf, const VisualizationParameters& p,
+	mrpt::opengl::CSetOfObjects& out)
+{
+	out.clear();
+
+	// Make a copy, to avoid potentially duplicated axis:
+	VisualizationParameters vp = p;
+
+	for (const auto& obs : sf)
+	{
+		if (!obs) continue;
+		auto glObj = mrpt::opengl::CSetOfObjects::Create();
+
+		bool ok = obs_to_viz(obs, vp, *glObj);
+		if (!ok) continue;
+
+		out.insert(glObj);
+
+		vp.showAxis = false;  // to avoid duplications
+	}
+
+	return !out.empty();
+}

libs/obs/include/mrpt/maps/CMetricMap.h

@@ -22,6 +22,23 @@
 
 #include <deque>
 
+#if defined(MRPT_BUILDING_PYTHON_WRAPPER)
+namespace mrpt::obs
+{
+class CSensoryFrame;
+}
+namespace mrpt::maps
+{
+class CMetricMap;
+}
+namespace mrpt::pymrpt_internal
+{
+bool insertObs(
+	const mrpt::obs::CSensoryFrame& sf, mrpt::maps::CMetricMap* map,
+	const mrpt::poses::CPose3D* robotPose);
+}
+#endif
+
 namespace mrpt::maps
 {
 /** Declares a virtual base class for all metric maps storage classes.
@@ -148,6 +165,12 @@ class CMetricMap : public mrpt::serialization::CSerializable,
 			return insertObservation(
 				obs, std::optional<const mrpt::poses::CPose3D>());
 	}
+	bool insertObs(
+		const mrpt::obs::CSensoryFrame& sf,
+		const mrpt::poses::CPose3D* robotPose = nullptr)
+	{
+		return mrpt::pymrpt_internal::insertObs(sf, this, robotPose);
+	}
 #endif
 
 	/** Computes the log-likelihood of a given observation given an arbitrary

python-examples/global_localization.py

@@ -72,17 +72,19 @@
         map_filename = os.path.abspath(map_filename)
         os.chdir(curr_dir)
 
-# options
-# kld
+# Load parameters:
+# KLD (Adapative sampling)
 pdf_prediction_options = mrpt.slam.TMonteCarloLocalizationParams()
 pdf_prediction_options.KLD_params.loadFromConfigFileName(
     config_filename, 'KLD_options')
 pdf_prediction_options.KLD_params.dumpToConsole()
-# pf
+
+# Particle filtering itself:
 pf_options = mrpt.bayes.CParticleFilter.TParticleFilterOptions()
 pf_options.loadFromConfigFileName(config_filename, 'PF_options')
 pf_options.dumpToConsole()
-# maps
+
+# Metric maps to build:
 map_list = mrpt.maps.TSetOfMetricMapInitializers()
 map_list.loadFromConfigFileName(config_filename, 'MetricMap')
 map_list.dumpToConsole()
@@ -91,20 +93,19 @@
 metric_map = mrpt.maps.CMultiMetricMap()
 metric_map.setListOfMaps(map_list)
 
-
 # load map
+map_file = mrpt.io.CFileGZInputStream(map_filename)
+map_arch = mrpt.serialization.archiveFrom(map_file)
+
 if (map_filename.endswith('.simplemap')
         or map_filename.endswith('.simplemap.gz')):
     simple_map = mrpt.maps.CSimpleMap()
-    map_file = mrpt.io.CFileGZInputStream(map_filename)
-    arch = mrpt.serialization.archiveFrom(map_file)
-    arch.ReadObject(simple_map)
+    map_arch.ReadObject(simple_map)
     metric_map.loadFromProbabilisticPosesAndObservations(simple_map)
 elif (map_filename.endswith('.gridmap')
         or map_filename.endswith('.gridmap.gz')):
     occ_map = mrpt.maps.COccupancyGridMap2D()
-    map_file = mrpt.utils.CFileGZInputStream(map_filename)
-    map_file.ReadObject(occ_map)
+    map_arch.ReadObject(occ_map)
     # overwrite gridmap
     for i in range(len(metric_map.maps)):
         if metric_map.maps[i].GetRuntimeClass().className == 'COccupancyGridMap2D':
@@ -115,7 +116,6 @@
     sys.exit(1)
 print('Load map file {}.'.format(map_filename))
 
-
 # get window resolution
 if args.resolution:
     resolution_str = args.resolution
@@ -124,13 +124,14 @@
 
 # gui
 try:
-    win3D = mrpt.gui.CDisplayWindow3D("pf_localization", int(
-        resolution_str.split('x')[0]), int(resolution_str.split('x')[1]))
+    res = resolution_str.split('x')
+    win3D = mrpt.gui.CDisplayWindow3D(
+        "pf_localization", int(res[0]), int(res[1]))
 except:
     win3D = mrpt.gui.CDisplayWindow3D("pf_localization", 800, 600)
 
 # initial scene
-map_object = metric_map.maps[0].getVisualization()
+map_object = metric_map.getVisualization()
 
 scene_ptr = win3D.get3DSceneAndLock()
 scene_ptr.clear()
@@ -169,23 +170,38 @@
     # get particles
     particles_object = pdf.getVisualization()
 
-    # get laserscan
-    points_map = mrpt.maps.CSimplePointsMap()
-    points_map.insertObs(sf.getObservationByIndex(0))
-    laserscan_object = points_map.getVisualization()
-    laserscan_object.setPose(mrpt.poses.CPose3D(mean))
-    laserscan_object.setColor(mrpt.img.TColorf(1., 0., 0.))
+    # get visualization for laser scan (two ways for demonstration purposes)
+    if False:
+        # Alternative method 1:
+        # Insert the observations into a point cloud:
+        points_map = mrpt.maps.CSimplePointsMap()
+        points_map.insertObs(sf)
 
-    # update pf
+        # And get the visualization of the point cloud:
+        glObservation = points_map.getVisualization()
+    else:
+        # Alternative 2:
+        # Using the generic obs_to_viz() method:
+        vizOpts = mrpt.obs.VisualizationParameters()
+        vizOpts.pointSize = 3
+        vizOpts.showAxis = False
+
+        glObservation = mrpt.opengl.CSetOfObjects()
+
+        mrpt.obs.obs_to_viz(sf, vizOpts, glObservation)
 
+    glObservation.setPose(mrpt.poses.CPose3D(mean))
+    glObservation.setColor(mrpt.img.TColorf(1., 0., 0.))
+
+    # update pf
     stats = pf.executeOn(pdf, act, sf)
 
     # update scene
     scene_ptr = win3D.get3DSceneAndLock()
     scene_ptr.clear()
     scene_ptr.insert(map_object)
     scene_ptr.insert(particles_object)
-    scene_ptr.insert(laserscan_object)
+    scene_ptr.insert(glObservation)
     win3D.unlockAccess3DScene()
     win3D.forceRepaint()
 

python/CMakeLists.txt

@@ -24,7 +24,10 @@ file(GLOB_RECURSE PY_SRCS ${PY_SRCS_DIR}/*.cpp)
 
 pybind11_add_module(pymrpt
   ${PY_SRCS}
+  # -- manually crafted files --
+  pymrpt_internals.cpp
   )
+
 target_link_libraries(pymrpt PUBLIC 
   mrpt::apps
   mrpt::serialization