Refactor of all particle-filter data structs

- mrpt::bayes::CProbabilityParticle (which affects all PF-based classes
in MRPT) has been greatly simplified via usage of the new
mrpt::utils::copy_ptr<> pointee-copy-semantics smart pointer.
- getCurrentMetricMapEstimation() renamed
mrpt::slam::CMultiMetricMapPDF::getAveragedMetricMapEstimation() to
avoid confusions

Author: jlblancoc

apps/rbpf-slam/rbpf-slam.cpp

@@ -225,7 +225,7 @@ void MapBuilding_RBPF()
 		mapBuilder.initialize(dummySimpleMap,&startPose);
 
 		for (CMultiMetricMapPDF::CParticleList::iterator it=mapBuilder.mapPDF.m_particles.begin();it!=mapBuilder.mapPDF.m_particles.end();++it) {
-			CRBPFParticleData* part_d = it->d;
+			CRBPFParticleData* part_d = it->d.get();
 			CMultiMetricMap &mmap = part_d->mapTillNow;
 			mrpt::maps::COccupancyGridMap2DPtr it_grid = mmap.getMapByClass<mrpt::maps::COccupancyGridMap2D>();
 			ASSERTMSG_(it_grid.present(), "No gridmap in multimetric map definition, but metric map continuation was set (!)" );
@@ -370,17 +370,17 @@ void MapBuilding_RBPF()
 
 			if (0==(step % LOG_FREQUENCY))
 			{
-				CMultiMetricMap		*mostLikMap = mapBuilder.mapPDF.getCurrentMostLikelyMetricMap();
+				const CMultiMetricMap *mostLikMap = mapBuilder.mapPDF.getCurrentMostLikelyMetricMap();
 
 				if (GENERATE_LOG_INFO)
 				{
-
 					printf("Saving info log information...");
 
 					tictac_JH.Tic();
 
-					ASSERT_( mapBuilder.getCurrentlyBuiltMetricMap()->m_gridMaps.size()>0 );
-					COccupancyGridMap2DPtr grid = mapBuilder.getCurrentlyBuiltMetricMap()->m_gridMaps[0];
+					const CMultiMetricMap * avrMap = mapBuilder.mapPDF.getAveragedMetricMapEstimation();
+					ASSERT_(avrMap->m_gridMaps.size()>0 );
+					COccupancyGridMap2DPtr grid = avrMap->m_gridMaps[0];
 					grid->computeEntropy( entropy );
 
 					grid->saveAsBitmapFile(format("%s/EMMI_gridmap_%03u.bmp",OUT_DIR,step));
@@ -600,7 +600,7 @@ void MapBuilding_RBPF()
 	filOut << finalMap;
 
 	// Save gridmap extend (if exists):
-	CMultiMetricMap		*mostLikMap = mapBuilder.mapPDF.getCurrentMostLikelyMetricMap();
+	const CMultiMetricMap *mostLikMap = mapBuilder.mapPDF.getCurrentMostLikelyMetricMap();
 	if (mostLikMap->m_gridMaps.size()>0)
 	{
 		CMatrix		auxMat(1,4);

apps/ro-localization/CPosePDFParticlesExtended.cpp

@@ -814,7 +814,7 @@ void  CPosePDFParticlesExtended::prediction_and_update_pfAuxiliaryPFOptimal(
 		for (i=0;i<M;i++)
 		{
 			oldParticles[i].log_w = m_particles[i].log_w;
-			oldParticles[i].d = new TExtendedCPose2D(*m_particles[i].d);
+			oldParticles[i].d.reset( new TExtendedCPose2D(*m_particles[i].d));
 		}
 
 
@@ -952,8 +952,8 @@ void  CPosePDFParticlesExtended::resetUniform(
 	{
 		clear();
 		m_particles.resize(particlesCount);
-		for (int i=0;i<particlesCount;i++)
-			m_particles[i].d = new TExtendedCPose2D();
+		for (int i = 0; i < particlesCount; i++)
+			m_particles[i].d.reset(new TExtendedCPose2D());
 	}
 
 	size_t		i,M = m_particles.size();

doc/doxygen-pages/changeLog_doc.h

@@ -52,9 +52,10 @@
 			- mrpt::poses::CRobot2DPoseEstimator now uses a more generic odometry-based velocity model (vx,vy,omega).
 			- New template mrpt::utils::ts_hash_map<> for thread-safe, std::map-like containers based on hash functions.
 			- Included exprtk header-only library to runtime compile & evaluation of mathematical expressions, under `<mrpt/otherlibs/exprtk.hpp>`
-			- New smart pointer templates: `mrpt::utils::copy_ptr<>`, `mrpt::utils::clone_ptr<>`.
+			- New smart pointer templates: `mrpt::utils::copy_ptr<>`, `mrpt::utils::poly_ptr<>`.
 		- \ref mrpt_bayes_grp
-			-  [API change] `verbose` is no longer a field of mrpt::bayes::CParticleFilter::TParticleFilterOptions. Use the setVerbosityLevel() method of the CParticleFilter class itself.
+			- [API change] `verbose` is no longer a field of mrpt::bayes::CParticleFilter::TParticleFilterOptions. Use the setVerbosityLevel() method of the CParticleFilter class itself.
+			- [API change] mrpt::bayes::CProbabilityParticle (which affects all PF-based classes in MRPT) has been greatly simplified via usage of the new mrpt::utils::copy_ptr<> pointee-copy-semantics smart pointer.
 		- \ref mrpt_gui_grp
 			- mrpt::gui::CMyGLCanvasBase is now derived from mrpt::opengl::CTextMessageCapable so they can draw text labels
 			- New class mrpt::gui::CDisplayWindow3DLocker for exception-safe 3D scene lock in 3D windows.
@@ -83,6 +84,7 @@
 			- [ABI change] mrpt::opengl::CAxis now has many new options exposed to configure its look.
 		- \ref mrpt_slam_grp
 			- [API change] mrpt::slam::CMetricMapBuilder::TOptions does not have a `verbose` field anymore. It's supersedded now by the verbosity level of the CMetricMapBuilder class itself.
+			- [API change] getCurrentMetricMapEstimation() renamed mrpt::slam::CMultiMetricMapPDF::getAveragedMetricMapEstimation() to avoid confusions.
 		- \ref mrpt_hwdrivers_grp
 			- mrpt::hwdrivers::CGenericSensor: external image format is now `png` by default instead of `jpg` to avoid losses.
 			- [ABI change] mrpt::hwdrivers::COpenNI2Generic:

libs/base/include/mrpt/bayes/CParticleFilterData.h

@@ -118,10 +118,7 @@ namespace bayes
 				for (j=lastIndxOld;j<sorted_idx;j++)
 				{
 					if (!oldParticlesReused[j])	/* If reused we can not delete that memory! */
-					{
-						delete derived().m_particles[j].d;
-						derived().m_particles[j].d = NULL;
-					}
+						derived().m_particles[j].d.reset();
 				}
 
 				/* For the next iteration:*/
@@ -139,23 +136,19 @@ namespace bayes
 				{
 					/* Make a copy of the particle's data: */
 					ASSERT_( derived().m_particles[ sorted_idx ].d != NULL);
-					parts[i].d = new typename Derived::CParticleDataContent( *derived().m_particles[ sorted_idx ].d );
+					parts[i].d.reset(new typename Derived::CParticleDataContent(*derived().m_particles[sorted_idx].d));
 				}
 			}
 			/* Free memory of unused particles */
-			for (itSrc=derived().m_particles.begin(),oldPartIt=oldParticlesReused.begin();itSrc!=derived().m_particles.end();itSrc++,oldPartIt++)
-				if (! *oldPartIt )
-				{
-					delete itSrc->d;
-					itSrc->d = NULL;
-				}
+			for (itSrc = derived().m_particles.begin(), oldPartIt = oldParticlesReused.begin(); itSrc != derived().m_particles.end(); itSrc++, oldPartIt++)
+				if (!*oldPartIt)
+					itSrc->d.reset();
 			/* Copy the pointers only to the final destination */
 			derived().m_particles.resize( parts.size() );
 			for (itSrc=parts.begin(),itDest=derived().m_particles.begin(); itSrc!=parts.end(); itSrc++, itDest++ )
 			{
 				itDest->log_w = itSrc->log_w;
-				itDest->d = itSrc->d;
-				itSrc->d = NULL;
+				itDest->d.move_from(itSrc->d);
 			}
 			parts.clear();
 			MRPT_END
@@ -184,24 +177,12 @@ namespace bayes
 		CParticleList  m_particles;	//!< The array of particles
 
 		/** Default constructor */
-		CParticleFilterData() : m_particles(0)
-		{ }
+		CParticleFilterData() : m_particles(0) {}
 
-		/** Free the memory of all the particles and reset the array "m_particles" to length zero.
-		  */
+		/** Free the memory of all the particles and reset the array "m_particles" to length zero  */
 		void clearParticles()
 		{
-			MRPT_START
-			for (typename CParticleList::iterator it=m_particles.begin();it!=m_particles.end();++it)
-				if (it->d) delete it->d;
 			m_particles.clear();
-			MRPT_END
-		}
-
-		/** Virtual destructor */
-		virtual ~CParticleFilterData()
-		{
-			clearParticles();
 		}
 
 		/** Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable).
@@ -234,7 +215,7 @@ namespace bayes
 			for (it=m_particles.begin();it!=m_particles.end();++it)
 			{
 				in >> it->log_w;
-				it->d = new T();
+				it->d.reset(new T());
 				in >> *it->d;
 			}
 			MRPT_END

libs/base/include/mrpt/bayes/CProbabilityParticle.h

@@ -9,6 +9,8 @@
 #ifndef CPROBABILITYPARTICLE_H
 #define CPROBABILITYPARTICLE_H
 
+#include <mrpt/utils/copy_ptr.h>
+
 namespace mrpt
 {
 namespace bayes
@@ -25,54 +27,11 @@ namespace bayes
 	struct CProbabilityParticle
 	{
 	public:
-		/** The data associated with this particle.
-		 */
-		T *d;
-
-		/** The (logarithmic) weight value for this particle.
-		 */
-		double log_w;
-
-		/** Default constructor:
-		 */
-		CProbabilityParticle() : d(NULL), log_w(0)
-		{
-		}
-
-		/** Copy constructor:
-		 */
-		CProbabilityParticle(const CProbabilityParticle &o) : d(NULL), log_w(o.log_w)
-		{
-			if (o.d)
-			{
-				// Copy
-				d = new T(*o.d);
-			}
-		}
+		mrpt::utils::copy_ptr<T> d;   //!< The data associated with this particle. The use of copy_ptr<> allows relying on compiler-generated copy ctor, etc.
+		double log_w; //!< The (logarithmic) weight value for this particle.
 
-		/** Copy operator
-		  */
- 		CProbabilityParticle<T> & operator =(const CProbabilityParticle &o)
-		{
-			if (this == &o) return *this;
-			log_w = o.log_w;
-			if (o.d)
-			{
-				// Copy semantic:
-				if (d)
-					*d = *o.d;			// Copy using the object "operator =".
-				else d = new T(*o.d);	// Create a new object from the copy constructor
-			}
-			else
-			{
-				if (d)
-				{
-					delete d;
-					d = NULL;
-				}
-			}
-			return *this;
-		}
+		/** Default constructor */
+		CProbabilityParticle() : d(), log_w(.0) {}
 	};
 
 	} // end namespace

libs/base/include/mrpt/poses/CPointPDFParticles.h

@@ -60,24 +60,16 @@ namespace poses
 		DEFINE_SERIALIZABLE( CPointPDFParticles )
 
 	 public:
-		/** Default constructor
-		  */
+		/** Default constructor */
 		CPointPDFParticles(size_t numParticles = 1);
 
-		/** Destructor
-		  */
-		virtual ~CPointPDFParticles();
-
-		/** Clear all the particles (free memory)
-		  */
-		void clear()  { setSize(0);	}
+		/** Clear all the particles (free memory) */
+		void clear();
 
-		/** Erase all the previous particles and change the number of particles, with a given initial value
-		  */
+		/** Erase all the previous particles and change the number of particles, with a given initial value  */
 		void setSize(size_t numberParticles, const CPoint3D &defaultValue = CPoint3D(0,0,0) );
 
-		/** Returns the number of particles
-		  */
+		/** Returns the number of particles */
 		size_t size() const
 		{
 			return m_particles.size();

libs/base/include/mrpt/poses/CPose3DPDFParticles.h

@@ -44,15 +44,6 @@ namespace mrpt
 			  */
 			CPose3DPDFParticles( size_t M = 1 );
 
-			/** Copy constructor */
-			inline CPose3DPDFParticles( const CPose3DPDFParticles& obj ) :
-				CPose3DPDF(),
-				CParticleFilterData<CPose3D>()
-			{
-				copyFrom( obj );
-			}
-			virtual ~CPose3DPDFParticles();//!< Destructor
-
 			void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE; //!< Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
 
 			/** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.

libs/base/include/mrpt/poses/CPosePDFParticles.h

@@ -39,26 +39,13 @@ namespace mrpt
 			DEFINE_SERIALIZABLE( CPosePDFParticles )
 
 		public:
-			/** Free all the memory associated to m_particles, and set the number of parts = 0
-			  */
-			void  clear();
+			void  clear(); //!< Free all the memory associated to m_particles, and set the number of parts = 0
 
 			/** Constructor
 			  * \param M The number of m_particles.
 			  */
 			CPosePDFParticles( size_t M = 1 );
 
-			/** Copy constructor:
-			  */
-			inline CPosePDFParticles( const CPosePDFParticles& obj )
-			{
-				copyFrom( obj );
-			}
-
-			/** Destructor
-			 */
-			virtual ~CPosePDFParticles();
-
 			/** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
 			  */
 			void  copyFrom(const CPosePDF &o) MRPT_OVERRIDE;

libs/base/src/poses/CPointPDFParticles.cpp

@@ -24,21 +24,14 @@ using namespace mrpt::system;
 IMPLEMENTS_SERIALIZABLE( CPointPDFParticles, CPointPDF, mrpt::poses )
 IMPLEMENTS_SERIALIZABLE( TSimple3DPoint, CSerializable, mrpt::poses )
 
-
-/*---------------------------------------------------------------
-		Constructor
-  ---------------------------------------------------------------*/
 CPointPDFParticles::CPointPDFParticles(size_t numParticles)
 {
 	setSize(numParticles);
 }
 
-/*---------------------------------------------------------------
-		Destructor
-  ---------------------------------------------------------------*/
-CPointPDFParticles::~CPointPDFParticles()
-{
-	clear();
+/** Clear all the particles (free memory) */
+void CPointPDFParticles::clear() { 
+	setSize(0); 
 }
 
 /*---------------------------------------------------------------
@@ -48,16 +41,12 @@ void CPointPDFParticles::setSize(
 	size_t	numberParticles,
 	const	CPoint3D &defaultValue)
 {
-	// Free old particles:
-	CParticleList::iterator	it;
-	for (it=m_particles.begin();it!=m_particles.end();++it)
-		delete it->d;
-
+	// Free old particles: automatic via smart ptr
 	m_particles.resize(numberParticles);
-	for (it=m_particles.begin();it!=m_particles.end();++it)
+	for (auto &it : m_particles)
 	{
-		it->log_w = 0;
-		it->d = new TSimple3DPoint(defaultValue);
+		it.log_w = 0;
+		it.d.reset(new TSimple3DPoint(defaultValue));
 	}
 }