10 #ifndef CRandomFieldGridMap2D_H
11 #define CRandomFieldGridMap2D_H
26 class COccupancyGridMap2D;
31 #if defined(MRPT_IS_X86_AMD64)
90 #if defined(MRPT_IS_X86_AMD64)
201 double x_max = 2,
double y_min = -2,
double y_max = 2,
202 double resolution = 0.1);
211 virtual bool isEmpty()
const override;
318 double new_x_min,
double new_x_max,
double new_y_min,
double new_y_max,
320 double additionalMarginMeters = 1.0f)
override;
330 const double x_min,
const double x_max,
const double y_min,
331 const double y_max,
const double resolution,
339 using Ptr = std::shared_ptr<ConnectivityDescriptor>;
353 size_t icx,
size_t icy,
355 size_t jcx,
size_t jcy,
358 double& out_edge_information) = 0;
411 const double sensorReading,
416 const bool update_map =
true,
419 const bool time_invariant =
true,
423 const double reading_stddev = .0);
439 double& out_predict_response,
441 double& out_predict_response_variance,
444 bool do_sensor_normalization,
559 void evalJacobian(
double& dr_dx_i,
double& dr_dx_j)
const override;
609 const bool update_map,
const bool time_invariant,
610 const double reading_information);
641 size_t cxo_max,
size_t cyo_min,
size_t cyo_max,
const size_t seed_cxo,
642 const size_t seed_cyo,
const size_t objective_cxo,
643 const size_t objective_cyo);
649 mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrKernelDM);
651 mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrKalmanFilter);
653 mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrKalmanApproximate);
655 mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrKernelDMV);
657 mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrGMRF_SD);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation, mrKernelDM)
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
#define MRPT_ENUM_TYPE_END()
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
This class allows loading and storing values and vectors of different types from a configuration text...
A 2D grid of dynamic size which stores any kind of data at each cell.
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.
void enableProfiler(bool enable=true)
bool isProfilerEnabled() const
A class for storing images as grayscale or RGB bitmaps.
Declares a virtual base class for all metric maps storage classes.
void clear()
Erase all the contents of the map.
A class for storing an occupancy grid map.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
void setMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD)
Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods).
std::deque< TPriorFactorGMRF > m_mrf_factors_priors
Vector with the precomputed priors for each GMRF model.
void enableVerbose(bool enable_verbose)
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
double m_average_normreadings_mean
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std ...
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
void setCellsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell
virtual CRandomFieldGridMap2D::TInsertionOptionsCommon * getCommonInsertOptions()=0
Get the part of the options common to all CRandomFieldGridMap2D classes.
bool isProfilerEnabled() const
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
The implementation in this class just calls all the corresponding method of the contained metric maps...
virtual void predictMeasurement(const double x, const double y, double &out_predict_response, double &out_predict_response_variance, bool do_sensor_normalization, const TGridInterpolationMethod interp_method=gimNearest)
Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form o...
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
void insertObservation_GMRF(double normReading, const mrpt::math::TPoint2D &point, const bool update_map, const bool time_invariant, const double reading_information)
The implementation of "insertObservation" for the Gaussian Markov Random Field map model.
virtual void internal_clear() override
Erase all the contents of the map.
void insertIndividualReading(const double sensorReading, const mrpt::math::TPoint2D &point, const bool update_map=true, const bool time_invariant=true, const double reading_stddev=.0)
Direct update of the map with a reading in a given position of the map, using the appropriate method ...
virtual void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
@ mrKalmanApproximate
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
@ mrGMRF_SD
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
@ mrKernelDMV
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
@ mrAchim
Another alias for "mrKernelDM", for backwards compatibility (see discussion in mrpt::maps::CRandomFie...
@ mrKalmanFilter
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D)
@ mrKernelDM
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D)
virtual bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
size_t m_average_normreadings_count
void clear()
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both bas...
void getAsMatlab3DGraphScript(std::string &out_script) const
Return a large text block with a MATLAB script to plot the contents of this map.
ConnectivityDescriptor::Ptr m_gmrf_connectivity
Empty: default.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
mrpt::graphs::ScalarFactorGraph m_gmrf
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
virtual void getAsBitmapFile(mrpt::img::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
double m_average_normreadings_var
virtual void getAsMatrix(mrpt::math::CMatrixDouble &out_mat) const
Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y...
bool isEnabledVerbose() const
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
bool m_rfgm_run_update_upon_clear
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell *fill_value=nullptr)
Changes the size of the grid, erasing previous contents.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
See docs in base class: in this class this always returns 0.
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
std::vector< float > m_DM_gaussWindow
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
void enableProfiler(bool enable=true)
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
float cell2float(const TRandomFieldCell &c) const override
virtual void saveAsMatlab3DGraph(const std::string &filName) const
Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the...
std::vector< std::list< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information.
void getMeanAndCov(mrpt::math::CVectorDouble &out_means, mrpt::math::CMatrixDouble &out_cov) const
Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based meth...
CRandomFieldGridMap2D(TMapRepresentation mapType=mrKernelDM, double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double resolution=0.1)
Constructor.
void getMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD) const
Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods).
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
virtual ~CRandomFieldGridMap2D()
Destructor.
void insertObservation_KernelDM_DMV(double normReading, const mrpt::math::TPoint2D &point, bool is_DMV)
The implementation of "insertObservation" for Achim Lilienthal's map models DM & DM+V.
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const TRandomFieldCell &defaultValueNewCells, double additionalMarginMeters=1.0f) override
Changes the size of the grid, maintaining previous contents.
bool exist_relation_between2cells(const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap, size_t cxo_min, size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo, const size_t seed_cyo, const size_t objective_cxo, const size_t objective_cyo)
Check if two cells of the gridmap (m_map) are connected, based on the provided occupancy gridmap.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
std::shared_ptr< CSetOfObjects > Ptr
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Versatile class for consistent logging and management of output messages.
VerbosityLevel getMinLoggingLevel() const
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
GLenum const GLfloat * params
GLsizei const GLchar ** string
GLsizei GLsizei GLchar * source
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned __int16 uint16_t
Simple, scalar (1-dim) constraint (edge) for a GMRF.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
virtual ~ConnectivityDescriptor()
virtual bool getEdgeInformation(const CRandomFieldGridMap2D *parent, size_t icx, size_t icy, size_t jcx, size_t jcy, double &out_edge_information)=0
Implement the check of whether node i=(icx,icy) is connected with node j=(jcx,jcy).
std::shared_ptr< ConnectivityDescriptor > Ptr
Parameters common to any derived class.
float cutoffRadius
The cutoff radius for updating cells.
size_t GMRF_gridmap_image_cy
Pixel coordinates of the origin for the occupancy_gridmap.
void internal_loadFromConfigFile_common(const mrpt::config::CConfigFileBase &source, const std::string §ion)
See utils::CLoadableOptions.
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
float sigma
The sigma of the "Parzen"-kernel Gaussian.
void internal_dumpToTextStream_common(std::ostream &out) const
See utils::CLoadableOptions.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
size_t GMRF_gridmap_image_cx
Pixel coordinates of the origin for the occupancy_gridmap.
float R_min
Limits for normalization of sensor readings.
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
bool GMRF_use_occupancy_information
whether to use information of an occupancy_gridmap map for building the GMRF
TInsertionOptionsCommon()
Default values loader.
double GMRF_saturate_min
(Default:-inf,+inf) Saturate the estimated mean in these limits
float KF_initialCellStd
The initial standard deviation of each cell's concentration (will be stored both at each cell's struc...
double GMRF_gridmap_image_res
occupancy_gridmap resolution: size of each pixel (m)
double GMRF_lambdaObs
The initial information (Lambda) of each observation (this information will decrease with time)
std::string GMRF_simplemap_file
simplemap_file name of the occupancy_gridmap
float KF_defaultCellMeanValue
The default value for the mean of cells' concentration.
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false)
double Lambda
"Information" of the observation (=inverse of the variance)
double evaluateResidual() const override
Return the residual/error of this observation.
double obsValue
Observation value.
TObservationGMRF(CRandomFieldGridMap2D &parent)
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
double getInformation() const override
Return the inverse of the variance of this constraint.
CRandomFieldGridMap2D * m_parent
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
double evaluateResidual() const override
Return the residual/error of this observation.
TPriorFactorGMRF(CRandomFieldGridMap2D &parent)
double getInformation() const override
Return the inverse of the variance of this constraint.
double Lambda
"Information" of the observation (=inverse of the variance)
CRandomFieldGridMap2D * m_parent
Parameters for CMetricMap::compute3DMatchingRatio()
The contents of each cell in a CRandomFieldGridMap2D map.
TRandomFieldCell(double kfmean_dm_mean=1e-20, double kfstd_dmmeanw=0)
Constructor.
double kf_mean
[KF-methods only] The mean value of this cell
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
double kf_std
[KF-methods only] The standard deviation value of this cell
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha
double updated_std
[Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve
double gmrf_mean
[GMRF only] The mean value of this cell
mrpt::system::TTimeStamp last_updated
[Dynamic maps only] The timestamp of the last time the cell was updated
double dm_mean
[Kernel-methods only] The cumulative weighted readings of this cell