text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          https://www.mrpt.org/                         |
     |                                                                        |
     | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
     | See: https://www.mrpt.org/Authors - All rights reserved.               |
     | Released under BSD License. See: https://www.mrpt.org/License          |
     +------------------------------------------------------------------------+ */
  #pragma once
  
  #include <mrpt/math/CMatrixF.h>
  #include <mrpt/obs/CObservation.h>  // INVALID_BEACON_ID
  #include <mrpt/poses/CPoint3D.h>
  #include <mrpt/poses/CPointPDFGaussian.h>
  #include <mrpt/poses/CPointPDFParticles.h>
  #include <mrpt/poses/CPointPDFSOG.h>
  #include <mrpt/serialization/CSerializable.h>
  
  #include <mrpt/opengl/CSetOfObjects.h>
  
  namespace mrpt::maps
  {
  class CBeaconMap;
  
  /** The class for storing individual "beacon landmarks" under a variety of 3D
   * position PDF distributions.
   *  This class is used for storage within the class CBeaconMap.
   *  The class implements the same methods than the interface "CPointPDF", and
   * invoking them actually becomes
   *   a mapping into the methods of the current PDF representation of the
   * beacon, selectable by means of "m_typePDF"
   * \sa CBeaconMap, CPointPDFSOG
   * \ingroup mrpt_maps_grp
   */
  class CBeacon : public mrpt::poses::CPointPDF
  {
  	DEFINE_SERIALIZABLE(CBeacon, mrpt::maps)
  
     public:
  	/** The type for the IDs of landmarks.
  	 */
  	using TBeaconID = int64_t;
  
  	/** See m_typePDF
  	 */
  	enum TTypePDF
  	{
  		pdfMonteCarlo = 0,
  		pdfGauss,
  		pdfSOG
  	};
  
  	/** Which one of the different 3D point PDF is currently used in this
  	 * object: montecarlo, gaussian, or a sum of gaussians.
  	 * \sa m_location
  	 */
  	TTypePDF m_typePDF{pdfGauss};
  
  	/** The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for
  	 * ease of use, but the CPointPDF interface is also implemented in CBeacon).
  	 */
  	mrpt::poses::CPointPDFParticles m_locationMC{1};
  	/** The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease
  	 * of use, but the CPointPDF interface is also implemented in CBeacon). */
  	mrpt::poses::CPointPDFGaussian m_locationGauss;
  	/** The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of
  	 * use, but the CPointPDF interface is also implemented in CBeacon). */
  	mrpt::poses::CPointPDFSOG m_locationSOG{1};
  
  	/** An ID for the landmark (see details next...)
  	 *  This ID was introduced in the version 3 of this class (21/NOV/2006),
  	 *and its aim is
  	 *  to provide a way for easily establishing correspondences between
  	 *landmarks detected
  	 *  in sequential image frames. Thus, the management of this field should
  	 *be:
  	 *		- In 'servers' (classes/modules/... that detect landmarks from
  	 *images):
  	 *A different ID must be assigned to every landmark (e.g. a sequential
  	 *counter), BUT only in the case of being sure of the correspondence of one
  	 *landmark with another one in the past (e.g. tracking).
  	 *		- In 'clients': This field can be ignored, but if it is used, the
  	 *advantage is solving the correspondence between landmarks detected in
  	 *consequentive instants of time: Two landmarks with the same ID
  	 *<b>correspond</b> to the same physical feature, BUT it should not be
  	 *expected the inverse to be always true.
  	 *
  	 * Note that this field is never fill out automatically, it must be set by
  	 *the programmer if used.
  	 */
  	TBeaconID m_ID{INVALID_BEACON_ID};
  
  	void getMean(mrpt::poses::CPoint3D& mean_point) const override;
  
  	std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
  
  	/** Copy operator, translating if necesary (for example, between particles
  	 * and gaussian representations) */
  	void copyFrom(const mrpt::poses::CPointPDF& o) override;
  
  	/** Save PDF's particles to a text file. See derived classes for more
  	 * information about the format of generated files */
  	bool saveToTextFile(const std::string& file) const override;
  
  	/** this = p (+) this. This can be used to convert a PDF from local
  	 * coordinates to global, providing the point (newReferenceBase) from which
  	 *   "to project" the current pdf. Result PDF substituted the currently
  	 * stored one in the object.
  	 */
  	void changeCoordinatesReference(
  		const mrpt::poses::CPose3D& newReferenceBase) override;
  
  	/** Saves a 3D representation of the beacon into a given OpenGL scene  */
  	void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const;
  
  	/** Gets a set of MATLAB commands which draw the current state of the
  	 * beacon: */
  	void getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const;
  
  	/** Draw a sample from the pdf. */
  	void drawSingleSample(mrpt::poses::CPoint3D& outSample) const override;
  
  	/** Bayesian fusion of two point distributions (product of two
  	 * distributions->new distribution), then save the result in this object
  	 * (WARNING: See implementing classes to see classes that can and cannot be
  	 * mixtured!)
  	 * \param p1 The first distribution to fuse
  	 * \param p2 The second distribution to fuse
  	 * \param minMahalanobisDistToDrop If set to different of 0, the result of
  	 * very separate Gaussian modes (that will result in negligible components)
  	 * in SOGs will be dropped to reduce the number of modes in the output.
  	 */
  	void bayesianFusion(
  		const CPointPDF& p1, const CPointPDF& p2,
  		const double minMahalanobisDistToDrop = 0) override;
  
  	/** Compute the observation model p(z_t|x_t) for a given observation (range
  	 * value), and return it as an approximate SOG.
  	 *  Note that if the beacon is a SOG itself, the number of gaussian modes
  	 * will be square.
  	 *  As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied
  	 * (maxDistanceFromCenter!=0), those modes farther than this sphere will be
  	 * discarded.
  	 *  Parameters such as the stdSigma of the sensor are gathered from
  	 * "myBeaconMap"
  	 *  The result is one "ring" for each Gaussian mode that represent the
  	 * beacon position in this object.
  	 *  The position of the sensor on the robot is used to shift the resulting
  	 * densities such as they represent the position of the robot, not the
  	 * sensor.
  	 *  \sa CBeaconMap::insertionOptions, generateRingSOG
  	 */
  	void generateObservationModelDistribution(
  		float sensedRange, mrpt::poses::CPointPDFSOG& outPDF,
  		const CBeaconMap* myBeaconMap,
  		const mrpt::poses::CPoint3D& sensorPntOnRobot,
  		const mrpt::poses::CPoint3D& centerPoint =
  			mrpt::poses::CPoint3D(0, 0, 0),
  		float maxDistanceFromCenter = 0) const;
  
  	/** This static method returns a SOG with ring-shape (or as a 3D sphere)
  	 * that can be used to initialize a beacon if observed the first time.
  	 *  sensorPnt is the center of the ring/sphere, i.e. the absolute position
  	 * of the range sensor.
  	 *  If clearPreviousContentsOutPDF=false, the SOG modes will be added to
  	 * the current contents of outPDF
  	 *  If the 3x3 matrix covarianceCompositionToAdd is provided, it will be
  	 * add to every Gaussian (to model the composition of uncertainty).
  	 * \sa generateObservationModelDistribution
  	 */
  	static void generateRingSOG(
  		float sensedRange, mrpt::poses::CPointPDFSOG& outPDF,
  		const CBeaconMap* myBeaconMap, const mrpt::poses::CPoint3D& sensorPnt,
  		const mrpt::math::CMatrixDouble33* covarianceCompositionToAdd = nullptr,
  		bool clearPreviousContentsOutPDF = true,
  		const mrpt::poses::CPoint3D& centerPoint =
  			mrpt::poses::CPoint3D(0, 0, 0),
  		float maxDistanceFromCenter = 0);
  
  };  // End of class definition
  
  }  // namespace mrpt::maps
  