10 #ifndef CRandomFieldGridMap2D_H 11 #define CRandomFieldGridMap2D_H 28 class COccupancyGridMap2D;
33 #if defined(MRPT_IS_X86_AMD64) 92 #if defined(MRPT_IS_X86_AMD64) 155 public mrpt::utils::COutputLogger
203 double x_max = 2,
double y_min = -2,
double y_max = 2,
204 double resolution = 0.1);
213 virtual bool isEmpty()
const override;
320 double new_x_min,
double new_x_max,
double new_y_min,
double new_y_max,
322 double additionalMarginMeters = 1.0f)
override;
332 const double x_min,
const double x_max,
const double y_min,
333 const double y_max,
const double resolution,
341 using Ptr = std::shared_ptr<ConnectivityDescriptor>;
355 size_t icx,
size_t icy,
357 size_t jcx,
size_t jcy,
360 double& out_edge_information) = 0;
413 const double sensorReading,
418 const bool update_map =
true,
421 const bool time_invariant =
true,
425 const double reading_stddev = .0);
441 double& out_predict_response,
443 double& out_predict_response_variance,
446 bool do_sensor_normalization,
474 this->setMinLoggingLevel(mrpt::utils::LVL_DEBUG);
478 return this->getMinLoggingLevel() == mrpt::utils::LVL_DEBUG;
561 void evalJacobian(
double& dr_dx_i,
double& dr_dx_j)
const override;
611 const bool update_map,
const bool time_invariant,
612 const double reading_information);
643 size_t cxo_max,
size_t cyo_min,
size_t cyo_max,
const size_t seed_cxo,
644 const size_t seed_cyo,
const size_t objective_cxo,
645 const size_t objective_cyo);
664 "mrKalmanApproximate");
void clear()
Erase all the contents of the map.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Simple, scalar (1-dim) constraint (edge) for a GMRF.
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Parameters for CMetricMap::compute3DMatchingRatio()
float sigma
The sigma of the "Parzen"-kernel Gaussian.
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)...
void clear()
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both bas...
static void fill(bimap< enum_t, std::string > &m_map)
double evaluateResidual() const override
Return the residual/error of this observation.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
unsigned __int16 uint16_t
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...
TInsertionOptionsCommon()
Default values loader.
CRandomFieldGridMap2D * m_parent
double m_average_normreadings_mean
float KF_defaultCellMeanValue
The default value for the mean of cells' concentration.
A class for storing images as grayscale or RGB bitmaps.
double getInformation() const override
Return the inverse of the variance of this constraint.
bool isProfilerEnabled() const
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...
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
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...
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
std::vector< std::list< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
void internal_loadFromConfigFile_common(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
See utils::CLoadableOptions.
void internal_dumpToTextStream_common(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
std::vector< float > m_DM_gaussWindow
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
double Lambda
"Information" of the observation (=inverse of the variance)
mrpt::system::TTimeStamp last_updated
[Dynamic maps only] The timestamp of the last time the cell was updated
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
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...
double gmrf_mean
[GMRF only] The mean value of this cell
Only specializations of this class are defined for each enum type of interest.
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
float cutoffRadius
The cutoff radius for updating cells.
void enableVerbose(bool enable_verbose)
Another alias for "mrKernelDM", for backwards compatibility (see discussion in mrpt::maps::CRandomFie...
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
void enableProfiler(bool enable=true)
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
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...
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).
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells' mean and std ...
This class allows loading and storing values and vectors of different types from a configuration text...
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
mrpt::graphs::ScalarFactorGraph m_gmrf
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.
TPriorFactorGMRF(CRandomFieldGridMap2D &parent)
virtual ~CRandomFieldGridMap2D()
Destructor.
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha
Parameters common to any derived class.
double getInformation() const override
Return the inverse of the variance of this constraint.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false) ...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
float R_min
Limits for normalization of sensor readings.
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
TRandomFieldCell(double kfmean_dm_mean=1e-20, double kfstd_dmmeanw=0)
Constructor.
virtual bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
A 2D grid of dynamic size which stores any kind of data at each cell.
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 isProfilerEnabled() const
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell ...
double kf_mean
[KF-methods only] The mean value of this cell
The contents of each cell in a CRandomFieldGridMap2D map.
double obsValue
Observation value.
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)...
size_t m_average_normreadings_count
CRandomFieldGridMap2D * m_parent
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
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.
std::shared_ptr< CSetOfObjects > Ptr
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.
float cell2float(const TRandomFieldCell &c) const override
virtual ~ConnectivityDescriptor()
double GMRF_lambdaObs
The initial information (Lambda) of each observation (this information will decrease with time) ...
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
ConnectivityDescriptor::Ptr m_gmrf_connectivity
Empty: default.
GLsizei const GLchar ** string
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
void setCellsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
double updated_std
[Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve ...
double GMRF_saturate_min
(Default:-inf,+inf) Saturate the estimated mean in these limits
TObservationGMRF(CRandomFieldGridMap2D &parent)
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
Declares a virtual base class for all metric maps storage classes.
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
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...
bool m_rfgm_run_update_upon_clear
virtual void internal_clear() override
Erase all the contents of the map.
virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
double m_average_normreadings_var
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
bool GMRF_use_occupancy_information
whether to use information of an occupancy_gridmap map for building the GMRF
std::string GMRF_simplemap_file
simplemap_file name of the occupancy_gridmap
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
GLsizei GLsizei GLchar * source
bool isEnabledVerbose() const
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
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...
std::shared_ptr< ConnectivityDescriptor > Ptr
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
virtual CRandomFieldGridMap2D::TInsertionOptionsCommon * getCommonInsertOptions()=0
Get the part of the options common to all CRandomFieldGridMap2D classes.
double evaluateResidual() const override
Return the residual/error of this observation.
double Lambda
"Information" of the observation (=inverse of the variance)
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
double dm_mean
[Kernel-methods only] The cumulative weighted readings of this cell
size_t GMRF_gridmap_image_cx
Pixel coordinates of the origin for the occupancy_gridmap.
std::deque< TPriorFactorGMRF > m_mrf_factors_priors
Vector with the precomputed priors for each GMRF model.
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
void enableProfiler(bool enable=true)
double GMRF_gridmap_image_res
occupancy_gridmap resolution: size of each pixel (m)
GLenum const GLfloat * params
float KF_initialCellStd
The initial standard deviation of each cell's concentration (will be stored both at each cell's struc...
size_t GMRF_gridmap_image_cy
Pixel coordinates of the origin for the occupancy_gridmap.
utils::CDynamicGrid< TRandomFieldCell > BASE
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...
double kf_std
[KF-methods only] The standard deviation value of this cell
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
void getAsMatlab3DGraphScript(std::string &out_script) const
Return a large text block with a MATLAB script to plot the contents of this 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 ...
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.
maps::CRandomFieldGridMap2D::TMapRepresentation enum_t
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.