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 CRejectionSamplingRangeOnlyLocalization_H
00029 #define CRejectionSamplingRangeOnlyLocalization_H
00030
00031 #include <mrpt/poses/CPose2D.h>
00032 #include <mrpt/poses/CPoint3D.h>
00033 #include <mrpt/poses/CPoint2D.h>
00034 #include <mrpt/bayes/CRejectionSamplingCapable.h>
00035
00036 namespace mrpt
00037 {
00038 namespace slam
00039 {
00040 using namespace mrpt::poses;
00041
00042 class CLandmarksMap;
00043 class CObservationBeaconRanges;
00044
00050 class MRPTDLLIMPEXP CRejectionSamplingRangeOnlyLocalization : public bayes::CRejectionSamplingCapable<poses::CPose2D>
00051 {
00052 public:
00055 CRejectionSamplingRangeOnlyLocalization();
00056
00059 virtual ~CRejectionSamplingRangeOnlyLocalization() { }
00060
00070 bool setParams(
00071 const CLandmarksMap &beaconsMap,
00072 const CObservationBeaconRanges &observation,
00073 float sigmaRanges,
00074 const CPose2D &oldPose,
00075 float robot_z = 0,
00076 bool autoCheckAngleRanges = true);
00077
00078 protected:
00081 void RS_drawFromProposal( CPose2D &outSample );
00082
00085 double RS_observationLikelihood( const CPose2D &x);
00086
00089 float m_z_robot;
00090
00091 float m_sigmaRanges;
00092 CPose2D m_oldPose;
00093
00096 size_t m_drawIndex;
00097
00100 struct MRPTDLLIMPEXP TDataPerBeacon
00101 {
00102 TDataPerBeacon() : sensorOnRobot(), beaconPosition(), radiusAtRobotPlane(0),minAngle(0),maxAngle(0)
00103 {}
00104
00105 CPoint3D sensorOnRobot;
00106 CPoint2D beaconPosition;
00107 float radiusAtRobotPlane;
00108 float minAngle,maxAngle;
00109 };
00110
00113 std::deque<TDataPerBeacon> m_dataPerBeacon;
00114
00115 };
00116
00117 }
00118 }
00119
00120 #endif