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 #ifndef CRangeBearingKFSLAM_H
00029 #define CRangeBearingKFSLAM_H
00030
00031 #include <mrpt/utils/CDebugOutputCapable.h>
00032 #include <mrpt/math/CVectorTemplate.h>
00033 #include <mrpt/math/CMatrixTemplateNumeric.h>
00034 #include <mrpt/utils/CConfigFileBase.h>
00035 #include <mrpt/utils/CLoadableOptions.h>
00036 #include <mrpt/opengl.h>
00037 #include <mrpt/bayes/CKalmanFilterCapable.h>
00038
00039 #include <mrpt/utils/safe_pointers.h>
00040
00041 #include <mrpt/slam/CSensoryFrame.h>
00042 #include <mrpt/slam/CActionCollection.h>
00043 #include <mrpt/slam/CObservationBearingRange.h>
00044 #include <mrpt/poses/CPoint3D.h>
00045 #include <mrpt/poses/CPose3DPDFGaussian.h>
00046 #include <mrpt/slam/CLandmark.h>
00047 #include <mrpt/slam/CSensFrameProbSequence.h>
00048 #include <mrpt/slam/CIncrementalMapPartitioner.h>
00049
00050 #include <map>
00051
00052 namespace mrpt
00053 {
00054 namespace slam
00055 {
00056 using namespace mrpt::bayes;
00057
00065 class MRPTDLLIMPEXP CRangeBearingKFSLAM : public utils::CDebugOutputCapable, public bayes::CKalmanFilterCapable
00066 {
00067 public:
00070 CRangeBearingKFSLAM( );
00071
00074 virtual ~CRangeBearingKFSLAM();
00075
00080 void processActionObservation(
00081 const CActionCollection &action,
00082 const CSensoryFrame &SF );
00083
00092 void getCurrentState(
00093 CPose3DPDFGaussian &out_robotPose,
00094 std::vector<CPoint3D> &out_landmarksPositions,
00095 std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
00096 CVectorDouble &out_fullState,
00097 CMatrixDouble &out_fullCovariance
00098 ) const;
00099
00103 void getCurrentRobotPose(
00104 CPose3DPDFGaussian &out_robotPose ) const;
00105
00109 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00110
00113 void loadOptions( const mrpt::utils::CConfigFileBase &ini );
00114
00117 struct MRPTDLLIMPEXP TOptions : utils::CLoadableOptions
00118 {
00121 TOptions();
00122
00125 void loadFromConfigFile(
00126 const mrpt::utils::CConfigFileBase &source,
00127 const std::string §ion);
00128
00131 void dumpToTextStream( CStream &out);
00132
00135 vector_float stds_Q_no_odo;
00136
00139 float std_sensor_range, std_sensor_yaw, std_sensor_pitch;
00140
00143 float std_odo_z_additional;
00144
00147 bool doPartitioningExperiment;
00148
00151 float quantiles_3D_representation;
00152
00157 int partitioningMethod;
00158
00159 } options;
00160
00165 void getLastPartition( std::vector<vector_uint> &parts )
00166 {
00167 parts = m_lastPartitionSet;
00168 }
00169
00176 void getLastPartitionLandmarks( std::vector<vector_uint> &landmarksMembership ) const;
00177
00180 void getLastPartitionLandmarksAsIfFixedSubmaps( size_t K, std::vector<vector_uint> &landmarksMembership );
00181
00182
00186 double computeOffDiagonalBlocksApproximationError( const std::vector<vector_uint> &landmarksMembership ) const;
00187
00188
00195 void reconsiderPartitionsNow();
00196
00197
00200 CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
00201 {
00202 return &mapPartitioner.options;
00203 }
00204
00207 void saveMapAndPath2DRepresentationAsMATLABFile(
00208 const std::string &fil,
00209 float stdCount=3.0f,
00210 const std::string &styleLandmarks = std::string("b"),
00211 const std::string &stylePath = std::string("r"),
00212 const std::string &styleRobot = std::string("r") ) const;
00213
00214
00215
00216 protected:
00217
00225 void OnGetAction( CVectorTemplate<KFTYPE> &out_u );
00226
00232 void OnTransitionModel(
00233 const CVectorTemplate<KFTYPE>& in_u,
00234 CVectorTemplate<KFTYPE>& inout_x,
00235 bool &out_skipPrediction
00236 );
00237
00242 void OnTransitionJacobian(
00243 CMatrixTemplateNumeric<KFTYPE> & out_F
00244 );
00245
00250 void OnTransitionNoise(
00251 CMatrixTemplateNumeric<KFTYPE> & out_Q
00252 );
00253
00260 void OnGetObservations(
00261 CMatrixTemplateNumeric<KFTYPE> &out_z,
00262 CVectorTemplate<KFTYPE> &out_R2,
00263 vector_int &out_data_association
00264 );
00265
00273 void OnGetObservations(
00274 CMatrixTemplateNumeric<KFTYPE> &out_z,
00275 CMatrixTemplateNumeric<KFTYPE> &out_R,
00276 vector_int &out_data_association
00277 )
00278 {
00279 CKalmanFilterCapable::OnGetObservations(out_z,out_R,out_data_association);
00280 }
00281
00282
00298 void OnObservationModelAndJacobians(
00299 const CMatrixTemplateNumeric<KFTYPE> &in_z,
00300 const vector_int &in_data_association,
00301 const bool &in_full,
00302 const int &in_obsIdx,
00303 CVectorTemplate<KFTYPE> &out_innov,
00304 CMatrixTemplateNumeric<KFTYPE> &out_Hx,
00305 CMatrixTemplateNumeric<KFTYPE> &out_Hy
00306 );
00307
00320 void OnInverseObservationModel(
00321 const CMatrixTemplateNumeric<KFTYPE> &in_z,
00322 const size_t &in_obsIdx,
00323 const size_t &in_idxNewFeat,
00324 CVectorTemplate<KFTYPE> &out_yn,
00325 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dxv,
00326 CMatrixTemplateNumeric<KFTYPE> &out_dyn_dhn );
00327
00328
00331 void OnNormalizeStateVector();
00332
00335 size_t get_vehicle_size() const { return 6; }
00336
00339 size_t get_feature_size() const { return 3; }
00340
00343 size_t get_observation_size() const { return 3; }
00344
00350 copiable_NULL_ptr<const CActionCollection> m_action;
00351
00354 copiable_NULL_ptr<const CSensoryFrame> m_SF;
00355
00358 std::map<CLandmark::TLandmarkID,unsigned int> m_IDs;
00359
00362 std::map<unsigned int,CLandmark::TLandmarkID> m_IDs_inverse;
00363
00364
00367 CIncrementalMapPartitioner mapPartitioner;
00368
00371 CSensFrameProbSequence m_SFs;
00372
00373 std::vector<vector_uint> m_lastPartitionSet;
00374
00377
00378
00379
00380 };
00381 }
00382 }
00383
00384 #endif