9 #ifndef CRangeBearingKFSLAM2D_H
10 #define CRangeBearingKFSLAM2D_H
61 void processActionObservation(
62 mrpt::obs::CActionCollectionPtr &action,
63 mrpt::obs::CSensoryFramePtr &SF );
75 std::vector<mrpt::math::TPoint2D> &out_landmarksPositions,
76 std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_landmarkIDs,
84 void getCurrentRobotPose(
90 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj )
const;
125 void saveMapAndPath2DRepresentationAsMATLABFile(
126 const std::string &fil,
128 const std::string &styleLandmarks = std::string(
"b"),
129 const std::string &stylePath = std::string(
"r"),
130 const std::string &styleRobot = std::string(
"r") )
const;
146 predictions_IDs.clear();
147 newly_inserted_landmarks.clear();
164 return m_last_data_association;
176 void OnGetAction( KFArray_ACT &out_u )
const;
183 void OnTransitionModel(
184 const KFArray_ACT &in_u,
185 KFArray_VEH &inout_x,
186 bool &out_skipPrediction
193 void OnTransitionJacobian( KFMatrix_VxV &out_F )
const;
197 void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments)
const;
204 void OnTransitionNoise( KFMatrix_VxV &out_Q )
const;
216 void OnGetObservationsAndDataAssociation(
217 vector_KFArray_OBS &out_z,
219 const vector_KFArray_OBS &in_all_predictions,
220 const KFMatrix &in_S,
222 const KFMatrix_OxO &in_R
225 void OnObservationModel(
227 vector_KFArray_OBS &out_predictions
235 void OnObservationJacobians(
236 const size_t &idx_landmark_to_predict,
243 void OnObservationJacobiansNumericGetIncrements(
244 KFArray_VEH &out_veh_increments,
245 KFArray_FEAT &out_feat_increments )
const;
250 void OnSubstractObservationVectors(KFArray_OBS &A,
const KFArray_OBS &B)
const;
255 void OnGetObservationNoise(KFMatrix_OxO &out_R)
const;
264 void OnPreComputingPredictions(
265 const vector_KFArray_OBS &in_all_prediction_means,
280 void OnInverseObservationModel(
281 const KFArray_OBS & in_z,
282 KFArray_FEAT & out_yn,
283 KFMatrix_FxV & out_dyn_dxv,
284 KFMatrix_FxO & out_dyn_dhn )
const;
291 void OnNewLandmarkAddedToMap(
292 const size_t in_obsIdx,
293 const size_t in_idxNewFeat );
298 void OnNormalizeStateVector();
304 void getLandmarkIDsFromIndexInStateVector(std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_id2index)
const
306 out_id2index = m_IDs.getInverseMap();
312 mrpt::obs::CActionCollectionPtr m_action;
315 mrpt::obs::CSensoryFramePtr m_SF;