text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          http://www.mrpt.org/                          |
     |                                                                        |
     | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
     | See: http://www.mrpt.org/Authors - All rights reserved.                |
     | Released under BSD License. See details in http://www.mrpt.org/License |
     +------------------------------------------------------------------------+ */
  #pragma once
  
  #include <mrpt/system/COutputLogger.h>
  #include <mrpt/containers/CThreadSafeQueue.h>
  
  #include <mrpt/hmtslam/HMT_SLAM_common.h>
  #include <mrpt/hmtslam/CLocalMetricHypothesis.h>
  #include <mrpt/hmtslam/CHierarchicalMHMap.h>
  #include <mrpt/hmtslam/CTopLCDetector_GridMatching.h>
  #include <mrpt/hmtslam/CTopLCDetector_FabMap.h>
  #include <mrpt/slam/CICP.h>
  #include <mrpt/maps/CPointsMap.h>
  #include <mrpt/slam/TKLDParams.h>
  #include <mrpt/obs/CActionCollection.h>
  #include <mrpt/opengl/COpenGLScene.h>
  #include <mrpt/serialization/CMessage.h>
  #include <mrpt/core/aligned_std_map.h>
  
  #include <thread>
  #include <queue>
  
  namespace mrpt
  {
  /** Classes related to the implementation of Hybrid Metric Topological (HMT)
   * SLAM. \ingroup mrpt_hmtslam_grp */
  namespace hmtslam
  {
  class CHMTSLAM;
  class CLSLAMAlgorithmBase;
  class CLSLAM_RBPF_2DLASER;
  
  /** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
   *
   *   The main entry points for a user are pushAction() and pushObservations().
   *Several parameters can be modified
   *   through m_options.
   *
   *  The mathematical models of this approach have been reported in:
   *		- Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J., "Towards a
   *Unified Bayesian Approach to Hybrid Metric-Topological SLAM", in  IEEE
   *Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
   *		- ...
   *
   *  More information in the wiki page: http://www.mrpt.org/HMT-SLAM . A complete
   *working application can be found in "MRPT/apps/hmt-slam".
   *
   *  The complete state of the SLAM framework is serializable, so it can be saved
   *and restore to/from a binary dump. This class implements
   *mrpt::serialization::CSerializable, so
   *    it can be saved with "stream << slam_object;" and restored with "stream >>
   *slam_object;". Alternatively, the methods CHMTSLAM::saveState and
   *CHMTSLAM::loadState
   *    can be invoked, which in turn call internally to the CSerializable
   *interface.
   *
   * \sa CHierarchicalMHMap
   * \ingroup mrpt_hmtslam_grp
   */
  class CHMTSLAM : public mrpt::system::COutputLogger,
  				 public mrpt::serialization::CSerializable
  {
  	friend class CLocalMetricHypothesis;
  	friend class CLSLAM_RBPF_2DLASER;
  	friend class CTopLCDetector_GridMatching;
  	friend class CTopLCDetector_FabMap;
  
  	DEFINE_SERIALIZABLE(CHMTSLAM)
  
     protected:
  	/** @name Inter-thread communication queues:
  		@{ */
  	/** Message definition:
  		- From: LSLAM
  		- To: AA
  		- Meaning: Reconsider the graph partition for the given LMH. Compute SSO
  	   for the new node(s) "newPoseIDs".
  		*/
  	/*struct TMessageAA
  	{
  		CLocalMetricHypothesis	*LMH;
  		TPoseIDList				newPoseIDs;
  	};*/
  
  	/** Message definition:
  		- From: AA
  		- To: LSLAM
  		- Meaning: The partitioning of robot poses.
  		*/
  	struct TMessageLSLAMfromAA
  	{
  		using Ptr = std::shared_ptr<TMessageLSLAMfromAA>;
  		/** The hypothesis ID (LMH) for these results. */
  		THypothesisID hypothesisID;
  		std::vector<TPoseIDList> partitions;
  
  		/** for debugging only */
  		void dumpToConsole() const;
  	};
  
  	/** Message definition:
  		- From: LSLAM
  		- To: TBI
  		- Meaning: One or more areas are to be considered by the TBI engines.
  		*/
  	struct TMessageLSLAMtoTBI
  	{
  		using Ptr = std::shared_ptr<TMessageLSLAMtoTBI>;
  		/** The LMH */
  		CLocalMetricHypothesis* LMH;
  		/** The areas to consider. */
  		TNodeIDList areaIDs;
  	};
  
  	/** Message definition:
  		- From: TBI
  		- To: LSLAM
  		- Meaning:
  		*/
  	struct TMessageLSLAMfromTBI
  	{
  		using Ptr = std::shared_ptr<TMessageLSLAMfromTBI>;
  		/** The hypothesis ID (LMH) for these results. */
  		THypothesisID hypothesisID;
  
  		/** The area for who the loop-closure has been computed. */
  		CHMHMapNode::TNodeID cur_area;
  
  		struct TBI_info
  		{
  			TBI_info() = default;
  			/** Log likelihood for this loop-closure. */
  			double log_lik{0};
  
  			/** Depending on the loop-closure engine, an guess of the relative
  			 * pose of "area_new" relative to "cur_area" is given here.
  			 *  If the SOG contains 0 modes, then the engine does not provide
  			 * this information.
  			 */
  			mrpt::poses::CPose3DPDFSOG delta_new_cur{0};
  		};
  
  		/** The meat is here: only feasible loop closures from "cur_area" are
  		 * included here, with associated data.
  		 */
  		std::map<CHMHMapNode::TNodeID, TBI_info> loopClosureData;
  
  		// MRPT_MAKE_ALIGNED_OPERATOR_NEW
  	};
  
  	/** LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA
  	 */
  	using CMessageQueue =
  		mrpt::containers::CThreadSafeQueue<mrpt::serialization::CMessage>;
  
  	CMessageQueue m_LSLAM_queue;
  
  	/** @} */
  
  	/** The Area Abstraction (AA) method, invoked from LSLAM.
  	 * \param LMH (IN) The LMH which to this query applies.
  	 * \param newPoseIDs (IN) The new poseIDs to be added to the graph
  	 * partitioner.
  	 * \return A structure with all return data. Memory to be freed by user.
  	 * \note The critical section for LMH must be locked BEFORE calling this
  	 * method (it does NOT lock any critical section).
  	 */
  	static TMessageLSLAMfromAA::Ptr areaAbstraction(
  		CLocalMetricHypothesis* LMH, const TPoseIDList& newPoseIDs);
  
  	/** The entry point for Topological Bayesian Inference (TBI) engines,
  	 * invoked from LSLAM.
  	 * \param LMH (IN) The LMH which to this query applies.
  	 * \param areaID (IN) The area ID to consider for potential loop-closures.
  	 * \note The critical section for LMH must be locked BEFORE calling this
  	 * method (it does NOT lock any critical section).
  	 */
  	static TMessageLSLAMfromTBI::Ptr TBI_main_method(
  		CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID);
  
  	/** @name Related to the input queue:
  		@{ */
     public:
  	/** Empty the input queue. */
  	void clearInputQueue();
  
  	/** Returns true if the input queue is empty (Note that the queue must not
  	 * be empty to the user to enqueue more actions/observaitions)
  	 * \sa pushAction,pushObservations, inputQueueSize
  	 */
  	bool isInputQueueEmpty();
  
  	/** Returns the number of objects waiting for processing in the input queue.
  	 * \sa pushAction,pushObservations, isInputQueueEmpty
  	 */
  	size_t inputQueueSize();
  
  	/** Here the user can enter an action into the system (will go to the SLAM
  	 * process).
  	 *  This class will delete the passed object when required, so DO NOT
  	 * DELETE the passed object after calling this.
  	 * \sa pushObservations,pushObservation
  	 */
  	void pushAction(const mrpt::obs::CActionCollection::Ptr& acts);
  
  	/** Here the user can enter observations into the system (will go to the
  	 * SLAM process).
  	 *  This class will delete the passed object when required, so DO NOT
  	 * DELETE the passed object after calling this.
  	 * \sa pushAction,pushObservation
  	 */
  	void pushObservations(const mrpt::obs::CSensoryFrame::Ptr& sf);
  
  	/** Here the user can enter an observation into the system (will go to the
  	 * SLAM process).
  	 *  This class will delete the passed object when required, so DO NOT
  	 * DELETE the passed object after calling this.
  	 * \sa pushAction,pushObservation
  	 */
  	void pushObservation(const mrpt::obs::CObservation::Ptr& obs);
  
  	enum TLSlamMethod
  	{
  		lsmRBPF_2DLASER = 1
  	};
  
     protected:
  	/** Used from the LSLAM thread to retrieve the next object from the queue.
  	 * \return The object, or nullptr if empty.
  	 */
  	mrpt::serialization::CSerializable::Ptr getNextObjectFromInputQueue();
  
  	/** The queue of pending actions/observations supplied by the user waiting
  	 * for being processed. */
  	std::queue<mrpt::serialization::CSerializable::Ptr> m_inputQueue;
  
  	/** Critical section for accessing  m_inputQueue */
  	mutable std::mutex m_inputQueue_cs;
  
  	/** Critical section for accessing m_map */
  	mutable std::mutex m_map_cs;
  
  	/** Critical section for accessing m_LMHs */
  	mutable std::mutex m_LMHs_cs;
  
  	/** @} */
  
  	/** @name Threads stuff
  		@{ */
  
  	/** The function for the "Local SLAM" thread. */
  	void thread_LSLAM();
  
  	/** The function for the "TBI" thread. */
  	void thread_TBI();
  
  	/** The function for the "3D viewer" thread. */
  	void thread_3D_viewer();
  	/** Threads handles */
  	std::thread m_hThread_LSLAM, m_hThread_TBI, m_hThread_3D_viewer;
  	/** @} */
  
  	/** @name HMT-SLAM sub-processes.
  		@{ */
  	/** Auxiliary method within thread_LSLAM */
  	void LSLAM_process_message(const mrpt::serialization::CMessage& msg);
  
  	/** No critical section locks are assumed at the entrance of this method.
  	 */
  	void LSLAM_process_message_from_AA(const TMessageLSLAMfromAA& myMsg);
  
  	/** No critical section locks are assumed at the entrance of this method.
  	 */
  	void LSLAM_process_message_from_TBI(const TMessageLSLAMfromTBI& myMsg);
  
  	/** Topological Loop Closure: Performs all the required operations
  		to close a loop between two areas which have been determined
  		to be the same.
  	 */
  	void perform_TLC(
  		CLocalMetricHypothesis& LMH, const CHMHMapNode::TNodeID areaInLMH,
  		const CHMHMapNode::TNodeID areaLoopClosure,
  		const mrpt::poses::CPose3DPDFGaussian& pose1wrt2);
  
  	/** @} */
  
  	/** @name The different SLAM algorithms that can be invoked from the LSLAM
  	   thread.
  		@{ */
  
  	/** An instance of a local SLAM method, to be applied to each LMH -
  	 * initialized by "initializeEmptyMap" or "loadState".
  	 */
  	CLSLAMAlgorithmBase* m_LSLAM_method;
  
  	/** @} */
  
  	/** @name The different Loop-Closure modules that are to be executed in the
  	   TBI thread.
  		@{ */
     protected:
  	using TLopLCDetectorFactory = CTopLCDetectorBase* (*)(CHMTSLAM*);
  
  	std::map<std::string, TLopLCDetectorFactory> m_registeredLCDetectors;
  
  	/** The list of LC modules in operation - initialized by
  	 * "initializeEmptyMap" or "loadState".  */
  	std::deque<CTopLCDetectorBase*> m_topLCdets;
  
  	/** The critical section for accessing m_topLCdets */
  	std::mutex m_topLCdets_cs;
  
     public:
  	/** Must be invoked before calling  initializeEmptyMap, so LC objects can be
  	 * created. */
  	void registerLoopClosureDetector(
  		const std::string& name,
  		CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*));
  
  	/** The class factory for topological loop closure detectors.
  	 *  Possible values are enumerated in TOptions::TLC_detectors
  	 *
  	 * \exception std::exception On unknown name.
  	 */
  	CTopLCDetectorBase* loopClosureDetector_factory(const std::string& name);
  
  	/** @} */
     protected:
  	/** Termination flag for signaling all threads to terminate */
  	bool m_terminateThreads;
  
  	/** Threads termination flags:
  	 */
  	bool m_terminationFlag_LSLAM, m_terminationFlag_TBI,
  		m_terminationFlag_3D_viewer;
  
  	/** Generates a new and unique area textual label (currently this generates
  	 * "0","1",...) */
  	static std::string generateUniqueAreaLabel();
  
  	/** Generates a new and unique pose ID */
  	static TPoseID generatePoseID();
  
  	/** Generates a new and unique hypothesis ID */
  	static THypothesisID generateHypothesisID();
  
  	static int64_t m_nextAreaLabel;
  	static TPoseID m_nextPoseID;
  	static THypothesisID m_nextHypID;
  
     public:
  	/** Default constructor
  	 *  \param debug_out_stream If debug output messages should be redirected
  	 * to any other stream apart from std::cout
  	 */
  	CHMTSLAM();
  
  	CHMTSLAM(const CHMTSLAM&) : mrpt::system::COutputLogger()
  	{
  		THROW_EXCEPTION("This object cannot be copied.");
  	}
  	const CHMTSLAM& operator=(const CHMTSLAM&)
  	{
  		THROW_EXCEPTION("This object cannot be copied.");
  	}
  
  	/** Destructor
  	 */
  	~CHMTSLAM() override;
  
  	/** Return true if an exception has been caught in any thread leading to the
  	 * end of the mapping application: no more actions/observations will be
  	 * processed from now on.
  	 */
  	bool abortedDueToErrors();
  
  	/** @name High-level map management
  		@{ */
  
  	/** Loads the options from a config file. */
  	void loadOptions(const std::string& configFile);
  	/** Loads the options from a config source */
  	void loadOptions(const mrpt::config::CConfigFileBase& cfgSource);
  
  	/** Initializes the whole HMT-SLAM framework, reseting to an empty map (It
  	 * also clears the logs directory) - this must be called AFTER loading the
  	 * options with CHMTSLAM::loadOptions. */
  	void initializeEmptyMap();
  
  	/** Save the state of the whole HMT-SLAM framework to some binary stream
  	 * (e.g. a file).
  	 * \return true if everything goes OK.
  	 * \sa loadState
  	 */
  	bool saveState(mrpt::serialization::CArchive& out) const;
  
  	/** Load the state of the whole HMT-SLAM framework from some binary stream
  	 * (e.g. a file).
  	 * \return true if everything goes OK.
  	 * \sa saveState
  	 */
  	bool loadState(mrpt::serialization::CArchive& in);
  	/** @} */
  
  	/** @name The important data.
  		@{ */
  	/** The hiearchical, multi-hypothesis graph-based map. */
  	CHierarchicalMHMap m_map;
  	/** The list of LMHs at each instant. */
  	mrpt::aligned_std_map<THypothesisID, CLocalMetricHypothesis> m_LMHs;
  	/** @} */
  
  	/** Called from LSLAM thread when log files must be created.
  	 */
  	void generateLogFiles(unsigned int nIteration);
  
  	/** Gets a 3D representation of the current state of the whole mapping
  	 * framework.
  	 */
  	void getAs3DScene(mrpt::opengl::COpenGLScene& outScene);
  
     protected:
  	/** A variety of options and configuration params (private, use
  	 * loadOptions).
  	 */
  	struct TOptions : public mrpt::config::CLoadableOptions
  	{
  		/** Initialization of default params
  		 */
  		TOptions();
  
  		void loadFromConfigFile(
  			const mrpt::config::CConfigFileBase& source,
  			const std::string& section) override;  // See base docs
  		void dumpToTextStream(
  			std::ostream& out) const override;  // See base docs
  
  		/** [LOGGING] If it is not an empty string (""), a directory with that
  		 * name will be created and log files save there. */
  		std::string LOG_OUTPUT_DIR;
  		/** [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log
  		 * file will be generated. */
  		int LOG_FREQUENCY;
  
  		/** [LSLAM] The method to use for local SLAM
  		 */
  		TLSlamMethod SLAM_METHOD;
  
  		/** [LSLAM] Minimum distance (and heading) difference between
  		 * observations inserted in the map.
  		 */
  		float SLAM_MIN_DIST_BETWEEN_OBS, SLAM_MIN_HEADING_BETWEEN_OBS;
  
  		/** [LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for
  		 * odometry increments (Default=0) */
  		float MIN_ODOMETRY_STD_XY;
  
  		/** [LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry
  		 * increments (Default=0) */
  		float MIN_ODOMETRY_STD_PHI;
  
  		/** [VIEW3D] The height of the areas' spheres.
  		 */
  		float VIEW3D_AREA_SPHERES_HEIGHT;
  
  		/** [VIEW3D] The radius of the areas' spheres.
  		 */
  		float VIEW3D_AREA_SPHERES_RADIUS;
  
  		/** A 3-length vector with the std. deviation of the transition model in
  		 * (x,y,phi) used only when there is no odometry (if there is odo, its
  		 * uncertainty values will be used instead); x y: In meters, phi:
  		 * radians (but in degrees when loading from a configuration ini-file!)
  		 */
  		mrpt::math::CVectorFloat stds_Q_no_odo;
  
  		/** [AA] The options for the partitioning algorithm
  		 */
  		mrpt::slam::CIncrementalMapPartitioner::TOptions AA_options;
  
  		/** The default set of maps to be created in each particle */
  		mrpt::maps::TSetOfMetricMapInitializers defaultMapsInitializers;
  		/** These params are used from every LMH object. */
  		bayes::CParticleFilter::TParticleFilterOptions pf_options;
  		mrpt::slam::TKLDParams KLD_params;
  
  		/** 0 means randomize, use any other value to have repetitive
  		 * experiments. */
  		int random_seed;
  
  		/** A list of topological loop-closure detectors to use: can be one or
  		 * more from this list:
  		 *  'gridmaps': Occupancy Grid matching.
  		 *  'fabmap': Mark Cummins' image matching framework.
  		 */
  		std::vector<std::string> TLC_detectors;
  
  		/** Options passed to this TLC constructor */
  		CTopLCDetector_GridMatching::TOptions TLC_grid_options;
  		/** Options passed to this TLC constructor */
  		CTopLCDetector_FabMap::TOptions TLC_fabmap_options;
  
  	} m_options;
  
  };  // End of class CHMTSLAM.
  
  /** Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
   */
  class CLSLAMAlgorithmBase
  {
  	friend class CLocalMetricHypothesis;
  
     protected:
  	mrpt::safe_ptr<CHMTSLAM> m_parent;
  
     public:
  	/** Constructor
  	 */
  	CLSLAMAlgorithmBase(CHMTSLAM* parent) : m_parent(parent) {}
  	/** Destructor
  	 */
  	virtual ~CLSLAMAlgorithmBase() = default;
  	/** Main entry point from HMT-SLAM: process some actions & observations.
  	 *  The passed action/observation will be deleted, so a copy must be made
  	 * if necessary.
  	 *  This method must be in charge of updating the robot pose estimates and
  	 * also to update the
  	 *   map when required.
  	 *
  	 * \param LMH   The local metric hypothesis which must be updated by this
  	 * SLAM algorithm.
  	 * \param act   The action to process (or nullptr).
  	 * \param sf    The observations to process (or nullptr).
  	 */
  	virtual void processOneLMH(
  		CLocalMetricHypothesis* LMH,
  		const mrpt::obs::CActionCollection::Ptr& act,
  		const mrpt::obs::CSensoryFrame::Ptr& sf) = 0;
  
  	/** The PF algorithm implementation.
  	 */
  	virtual void prediction_and_update_pfAuxiliaryPFOptimal(
  		CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
  		const mrpt::obs::CSensoryFrame* observation,
  		const bayes::CParticleFilter::TParticleFilterOptions& PF_options) = 0;
  
  	/** The PF algorithm implementation.  */
  	virtual void prediction_and_update_pfOptimalProposal(
  		CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
  		const mrpt::obs::CSensoryFrame* observation,
  		const bayes::CParticleFilter::TParticleFilterOptions& PF_options) = 0;
  
  };  // end of class CLSLAMAlgorithmBase
  
  /** Implements a 2D local SLAM method based on a RBPF over an occupancy grid
   * map.
   *  This class is used internally in mrpt::slam::CHMTSLAM
   */
  class CLSLAM_RBPF_2DLASER : public CLSLAMAlgorithmBase
  {
  	friend class CLocalMetricHypothesis;
  
     public:
  	/** Constructor
  	 */
  	CLSLAM_RBPF_2DLASER(CHMTSLAM* parent);
  
  	/** Destructor
  	 */
  	~CLSLAM_RBPF_2DLASER() override;
  
  	/** Main entry point from HMT-SLAM: process some actions & observations.
  	 *  The passed action/observation will be deleted, so a copy must be made
  	 * if necessary.
  	 *  This method must be in charge of updating the robot pose estimates and
  	 * also to update the
  	 *   map when required.
  	 *
  	 * \param LMH   The local metric hypothesis which must be updated by this
  	 * SLAM algorithm.
  	 * \param act   The action to process (or nullptr).
  	 * \param sf    The observations to process (or nullptr).
  	 */
  	void processOneLMH(
  		CLocalMetricHypothesis* LMH,
  		const mrpt::obs::CActionCollection::Ptr& act,
  		const mrpt::obs::CSensoryFrame::Ptr& sf) override;
  
  	/** The PF algorithm implementation.  */
  	void prediction_and_update_pfAuxiliaryPFOptimal(
  		CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
  		const mrpt::obs::CSensoryFrame* observation,
  		const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
  		override;
  
  	/** The PF algorithm implementation.  */
  	void prediction_and_update_pfOptimalProposal(
  		CLocalMetricHypothesis* LMH, const mrpt::obs::CActionCollection* action,
  		const mrpt::obs::CSensoryFrame* observation,
  		const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
  		override;
  
     protected:
  	/**  For use within PF callback methods */
  	bool m_insertNewRobotPose;
  
  	/** Auxiliary structure
  	 */
  	struct TPathBin
  	{
  		TPathBin() : x(), y(), phi() {}
  		std::vector<int> x, y, phi;
  
  		/** For debugging purposes!
  		 */
  		void dumpToStdOut() const;
  	};
  
  	/** Fills out a "TPathBin" variable, given a path hypotesis and (if not set
  	 * to nullptr) a new pose appended at the end, using the KLD params in
  	 * "options".
  	 */
  	void loadTPathBinFromPath(
  		TPathBin& outBin, TMapPoseID2Pose3D* path = nullptr,
  		mrpt::poses::CPose2D* newPose = nullptr);
  
  	/** Checks if a given "TPathBin" element is already into a set of them, and
  	 * return its index (first one is 0), or -1 if not found.
  	 */
  	int findTPathBinIntoSet(TPathBin& desiredBin, std::deque<TPathBin>& theSet);
  
  	/** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
  	 */
  	static double particlesEvaluator_AuxPFOptimal(
  		const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
  		const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
  		const void* action, const void* observation);
  
  	/** Auxiliary function that evaluates the likelihood of an observation,
  	 * given a robot pose, and according to the options in
  	 * "CPosePDFParticles::options".
  	 */
  	static double auxiliarComputeObservationLikelihood(
  		const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
  		const mrpt::bayes::CParticleFilterCapable* obj,
  		size_t particleIndexForMap, const mrpt::obs::CSensoryFrame* observation,
  		const mrpt::poses::CPose2D* x);
  
  };  // end class CLSLAM_RBPF_2DLASER
  
  }  // namespace hmtslam
  }  // namespace mrpt
  