10 #ifndef CRandomFieldGridMap2D_H 11 #define CRandomFieldGridMap2D_H 29 class COccupancyGridMap2D;
34 #if defined(MRPT_IS_X86_AMD64) 45 kf_mean (kfmean_dm_mean),
46 kf_std (kfstd_dmmeanw),
48 last_updated(
mrpt::system::
now()),
49 updated_std (kfstd_dmmeanw)
80 #if defined(MRPT_IS_X86_AMD64) 117 public mrpt::utils::COutputLogger
152 double x_min = -2,
double x_max = 2,
153 double y_min = -2,
double y_max = 2,
154 double resolution = 0.1
173 virtual
void saveAsBitmapFile(const
std::
string &filName) const;
176 virtual
void getAsBitmapFile(
mrpt::utils::CImage &out_img) const;
188 TInsertionOptionsCommon();
191 void internal_loadFromConfigFile_common(
193 const std::string §ion);
233 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 )
MRPT_OVERRIDE;
239 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 = NULL);
245 virtual ~ConnectivityDescriptor();
251 virtual bool getEdgeInformation(
253 size_t icx,
size_t icy,
254 size_t jcx,
size_t jcy,
255 double &out_edge_information
261 void setCellsConnectivity(
const ConnectivityDescriptorPtr &new_connectivity_descriptor);
267 virtual void saveMetricMapRepresentationToFile(
const std::string &filNamePrefix)
const MRPT_OVERRIDE;
272 virtual void saveAsMatlab3DGraph(
const std::string &filName)
const;
276 void getAsMatlab3DGraphScript(std::string &out_script)
const;
279 virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj )
const MRPT_OVERRIDE;
282 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &meanObj, mrpt::opengl::CSetOfObjectsPtr &varObj )
const;
291 void insertIndividualReading(
292 const double sensorReading,
294 const bool update_map =
true,
295 const bool time_invariant =
true,
296 const double reading_stddev = .0
305 virtual void predictMeasurement(
308 double &out_predict_response,
309 double &out_predict_response_variance,
310 bool do_sensor_normalization,
323 void updateMapEstimation();
325 void enableVerbose(
bool enable_verbose) { this->setMinLoggingLevel(mrpt::utils::LVL_DEBUG); }
326 bool isEnabledVerbose()
const {
return this->getMinLoggingLevel()== mrpt::utils::LVL_DEBUG; }
369 double evaluateResidual() const MRPT_OVERRIDE;
370 double getInformation() const MRPT_OVERRIDE;
371 void evalJacobian(
double &dr_dx) const MRPT_OVERRIDE;
382 double evaluateResidual() const MRPT_OVERRIDE;
383 double getInformation() const MRPT_OVERRIDE;
384 void evalJacobian(
double &dr_dx_i,
double &dr_dx_j) const MRPT_OVERRIDE;
400 void insertObservation_KernelDM_DMV(
409 void insertObservation_KF(
417 void insertObservation_KF2(
425 void insertObservation_GMRF(
double normReading,
const mrpt::math::TPoint2D &point,
const bool update_map,
const bool time_invariant,
const double reading_information);
428 void updateMapEstimation_GMRF();
431 double computeConfidenceCellValue_DM_DMV (
const TRandomFieldCell *cell )
const;
442 void recoverMeanAndCov()
const;
445 virtual void internal_clear() MRPT_OVERRIDE;
448 bool exist_relation_between2cells(
454 const
size_t seed_cxo,
455 const
size_t seed_cyo,
456 const
size_t objective_cxo,
457 const
size_t objective_cyo);
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.
ConnectivityDescriptorPtr m_gmrf_connectivity
Empty: default.
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)
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
CRandomFieldGridMap2D * m_parent
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
float KF_defaultCellMeanValue
The default value for the mean of cells' concentration.
bool isProfilerEnabled() const
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
void clear()
Erase all the contents of the map.
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.
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.
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)
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
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)...
Parameters common to any derived class.
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...
TRandomFieldCell(double kfmean_dm_mean=1e-20, double kfstd_dmmeanw=0)
Constructor.
A 2D grid of dynamic size which stores any kind of data at each cell.
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
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.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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...
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
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...
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
double updated_std
[Dynamic maps only] The std cell value that was updated (to be used in the Forgetting_curve ...
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).
bool m_rfgm_run_update_upon_clear
stlplus::smart_ptr< ConnectivityDescriptor > ConnectivityDescriptorPtr
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.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
bool isEnabledVerbose() const
float cell2float(const TRandomFieldCell &c) const MRPT_OVERRIDE
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
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.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
void enableProfiler(bool enable=true)
double GMRF_gridmap_image_res
occupancy_gridmap resolution: size of each pixel (m)
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
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.
maps::CRandomFieldGridMap2D::TMapRepresentation enum_t
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.