00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef CHeightGridMap2D_H
00030 #define CHeightGridMap2D_H
00031
00032 #include <mrpt/utils/CDynamicGrid.h>
00033 #include <mrpt/utils/CSerializable.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 #include <mrpt/system/os.h>
00036 #include <mrpt/utils/CLoadableOptions.h>
00037 #include <mrpt/utils/stl_extensions.h>
00038
00039 #include <mrpt/slam/CMetricMap.h>
00040
00041 namespace mrpt
00042 {
00043 namespace poses
00044 {
00045 class CPose2D;
00046 }
00047 namespace slam
00048 {
00049 using namespace mrpt::utils;
00050
00051 class CObservation;
00052
00053 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CHeightGridMap2D, CMetricMap )
00054
00055
00057 struct MRPTDLLIMPEXP THeightGridmapCell
00058 {
00061 THeightGridmapCell() : h(0), w(0), history_Zs()
00062 {}
00063
00066 float h;
00067
00070 float var;
00071
00074 float u;
00075
00078 float v;
00079
00080
00083 uint32_t w;
00084
00087 multimap_serializable<mrpt::system::TTimeStamp,float> history_Zs;
00088 };
00089
00097 class MRPTDLLIMPEXP CHeightGridMap2D : public CMetricMap, public utils::CDynamicGrid<THeightGridmapCell>
00098 {
00099
00100 DEFINE_SERIALIZABLE( CHeightGridMap2D )
00101 public:
00102
00103 float cell2float(const THeightGridmapCell& c) const
00104 {
00105 return float(c.h);
00106 }
00107
00111 enum TMapRepresentation
00112 {
00113 mrSimpleAverage = 0,
00114 mrSlidingWindow
00115 };
00116
00117
00120 CHeightGridMap2D(
00121 TMapRepresentation mapType = mrSimpleAverage,
00122 float x_min = -2,
00123 float x_max = 2,
00124 float y_min = -2,
00125 float y_max = 2,
00126 float resolution = 0.1
00127 );
00128
00131 void clear();
00132
00135 bool isEmpty() const;
00136
00144 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00145
00154 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00155
00159 size_t removeObservationsByTimestamp( const mrpt::system::TTimeStamp &tim );
00160
00163 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions
00164 {
00167 TInsertionOptions();
00168
00171 void loadFromConfigFile(
00172 const mrpt::utils::CConfigFileBase &source,
00173 const std::string §ion);
00174
00177 void dumpToTextStream(utils::CStream &out);
00178
00181 bool filterByHeight;
00182
00185 float z_min,z_max;
00186
00187 float minDistBetweenPointsWhenInserting;
00188
00189 } insertionOptions;
00190
00201 float compute3DMatchingRatio(
00202 const CMetricMap *otherMap,
00203 const CPose3D &otherMapPose,
00204 float minDistForCorr = 0.10f,
00205 float minMahaDistForCorr = 2.0f
00206 ) const;
00207
00210 void saveMetricMapRepresentationToFile(
00211 const std::string &filNamePrefix
00212 ) const;
00213
00216 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00217
00221 void auxParticleFilterCleanUp();
00222
00225 TMapRepresentation getMapType();
00226
00227 protected:
00228
00231 TMapRepresentation m_mapType;
00232
00233
00234 };
00235
00236
00237 }
00238 }
00239
00240 #endif