This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
It has a set of particles representing the robot path in nearby poses.
Definition at line 92 of file CLocalMetricHypothesis.h.
#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
Classes | |
struct | TRobotPosesPartitioning |
Used by AA thread. More... | |
Public Member Functions | |
CLocalMetricHypothesis (CHMTSLAM *parent=NULL) | |
Constructor (Default param only used from STL classes) | |
~CLocalMetricHypothesis () | |
Destructor. | |
void | getAs3DScene (mrpt::opengl::CSetOfObjectsPtr &objs) const |
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph, and each of the areas they belong to. | |
void | getMeans (std::map< TPoseID, CPose3D > &outList) const |
Returns the mean of each robot pose in this LMH, as computed from the set of particles. | |
void | getPathParticles (std::map< TPoseID, CPose3DPDFParticles > &outList) const |
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles. | |
void | getPoseParticles (const TPoseID &poseID, CPose3DPDFParticles &outPDF) const |
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles. | |
void | getRelativePose (const TPoseID &reference, const TPoseID &pose, CPose3DPDFParticles &outPDF) const |
Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH). | |
void | dumpAsText (utils::CStringList &st) const |
Describes the LMH in text. | |
void | changeCoordinateOrigin (const TPoseID &newOrigin) |
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric maps and change coords in the partitioning subsystem as well. | |
void | rebuildMetricMaps () |
Rebuild the metric maps of all particles from the observations and their estimated poses. | |
void | clearRobotPoses () |
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses. | |
const CPose3D * | getCurrentPose (const size_t &particleIdx) const |
Returns the i'th particle hypothesis for the current robot pose. | |
CPose3D * | getCurrentPose (const size_t &particleIdx) |
Returns the i'th particle hypothesis for the current robot pose. | |
void | removeAreaFromLMH (const CHMHMapNode::TNodeID areaID) |
Removes a given area from the LMH:
| |
void | updateAreaFromLMH (const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH=false) |
The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are referenced to the area's reference poseID, such as that reference is at the origin. | |
Public Attributes | |
synch::CCriticalSection | m_lock |
Critical section for threads signaling they are working with the LMH. | |
THypothesisID | m_ID |
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap). | |
safe_ptr< CHMTSLAM > | m_parent |
For quick access to our parent object. | |
TPoseID | m_currentRobotPose |
The current robot pose (its global unique ID) for this hypothesis. | |
TNodeIDSet | m_neighbors |
The list of all areas sourronding the current one (this includes the current area itself). | |
std::map< TPoseID, CHMHMapNode::TNodeID > | m_nodeIDmemberships |
The hybrid map node membership for each robot pose. | |
std::map< TPoseID, CSensoryFrame > | m_SFs |
The SF gathered at each robot pose. | |
TPoseIDList | m_posesPendingAddPartitioner |
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop. | |
TNodeIDList | m_areasPendingTBI |
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to search for potential loop-closures. Set in CHMTSLAM::LSLAM_process_message_from_AA, read in. | |
double | m_log_w |
Log-weight of this hypothesis. | |
std::vector< std::map< TPoseID, double > > | m_log_w_metric_history |
The historic log-weights of the metric observations inserted in this LMH, for each particle. | |
CActionRobotMovement2D | m_accumRobotMovement |
Used in CLSLAM_RBPF_2DLASER. | |
bool | m_accumRobotMovementIsValid |
Used in CLSLAM_RBPF_2DLASER. | |
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning | m_robotPosesGraph |
Protected Member Functions | |
Virtual methods for Particle Filter implementation (just a wrapper interface, actually implemented in CHMTSLAM::m_LSLAM_method) | |
void | prediction_and_update_pfAuxiliaryPFOptimal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
void | prediction_and_update_pfOptimalProposal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
Protected Attributes | |
vector_double | m_pfAuxiliaryPFOptimal_estimatedProb |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
std::vector< double > | m_maxLikelihood |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
std::vector< CPose2D, Eigen::aligned_allocator < CPose2D > > | m_movementDraws |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
unsigned int | m_movementDrawsIdx |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
StdVector_CPose2D | m_movementDrawMaximumLikelihood |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
Friends | |
class HMTSLAM_IMPEXP | CLSLAM_RBPF_2DLASER |
RTTI stuff | |
typedef CLocalMetricHypothesisPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CLocalMetricHypothesis |
static mrpt::utils::TRuntimeClassId | classCLocalMetricHypothesis |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const |
Returns information about the class of an object in runtime. | |
virtual mrpt::utils::CObject * | duplicate () const |
Returns a copy of the object, indepently of its class. | |
static mrpt::utils::CObject * | CreateObject () |
static CLocalMetricHypothesisPtr | Create () |
A typedef for the associated smart pointer
Definition at line 99 of file CLocalMetricHypothesis.h.
mrpt::hmtslam::CLocalMetricHypothesis::CLocalMetricHypothesis | ( | CHMTSLAM * | parent = NULL ) |
Constructor (Default param only used from STL classes)
mrpt::hmtslam::CLocalMetricHypothesis::~CLocalMetricHypothesis | ( | ) |
Destructor.
static const mrpt::utils::TRuntimeClassId* mrpt::hmtslam::CLocalMetricHypothesis::_GetBaseClass | ( | ) | [static, protected] |
Reimplemented from mrpt::utils::CSerializable.
void mrpt::hmtslam::CLocalMetricHypothesis::changeCoordinateOrigin | ( | const TPoseID & | newOrigin ) |
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric maps and change coords in the partitioning subsystem as well.
void mrpt::hmtslam::CLocalMetricHypothesis::clearRobotPoses | ( | ) |
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses.
Sets the number of particles to the initial number according to the PF options, and initialize them with no robot poses & empty metric maps.
static CLocalMetricHypothesisPtr mrpt::hmtslam::CLocalMetricHypothesis::Create | ( | ) | [static] |
static mrpt::utils::CObject* mrpt::hmtslam::CLocalMetricHypothesis::CreateObject | ( | ) | [static] |
void mrpt::hmtslam::CLocalMetricHypothesis::dumpAsText | ( | utils::CStringList & | st ) | const |
Describes the LMH in text.
virtual mrpt::utils::CObject* mrpt::hmtslam::CLocalMetricHypothesis::duplicate | ( | ) | const [virtual] |
Returns a copy of the object, indepently of its class.
Implements mrpt::utils::CObject.
void mrpt::hmtslam::CLocalMetricHypothesis::getAs3DScene | ( | mrpt::opengl::CSetOfObjectsPtr & | objs ) | const |
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph, and each of the areas they belong to.
The metric maps are *not* included here for convenience, call m_metricMaps.getAs3DScene(). The previous contents of "objs" will be discarded
CPose3D* mrpt::hmtslam::CLocalMetricHypothesis::getCurrentPose | ( | const size_t & | particleIdx ) |
Returns the i'th particle hypothesis for the current robot pose.
const CPose3D* mrpt::hmtslam::CLocalMetricHypothesis::getCurrentPose | ( | const size_t & | particleIdx ) | const |
Returns the i'th particle hypothesis for the current robot pose.
void mrpt::hmtslam::CLocalMetricHypothesis::getMeans | ( | std::map< TPoseID, CPose3D > & | outList ) | const |
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
void mrpt::hmtslam::CLocalMetricHypothesis::getPathParticles | ( | std::map< TPoseID, CPose3DPDFParticles > & | outList ) | const |
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
void mrpt::hmtslam::CLocalMetricHypothesis::getPoseParticles | ( | const TPoseID & | poseID, |
CPose3DPDFParticles & | outPDF | ||
) | const |
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.
void mrpt::hmtslam::CLocalMetricHypothesis::getRelativePose | ( | const TPoseID & | reference, |
const TPoseID & | pose, | ||
CPose3DPDFParticles & | outPDF | ||
) | const |
Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH).
virtual const mrpt::utils::TRuntimeClassId* mrpt::hmtslam::CLocalMetricHypothesis::GetRuntimeClass | ( | ) | const [virtual] |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::utils::CSerializable.
void mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update_pfAuxiliaryPFOptimal | ( | const mrpt::slam::CActionCollection * | action, |
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [protected, virtual] |
The PF algorithm implementation.
Reimplemented from mrpt::bayes::CParticleFilterCapable.
void mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update_pfOptimalProposal | ( | const mrpt::slam::CActionCollection * | action, |
const mrpt::slam::CSensoryFrame * | observation, | ||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | ||
) | [protected, virtual] |
The PF algorithm implementation.
Reimplemented from mrpt::bayes::CParticleFilterCapable.
void mrpt::hmtslam::CLocalMetricHypothesis::rebuildMetricMaps | ( | ) |
Rebuild the metric maps of all particles from the observations and their estimated poses.
void mrpt::hmtslam::CLocalMetricHypothesis::removeAreaFromLMH | ( | const CHMHMapNode::TNodeID | areaID ) |
Removes a given area from the LMH:
The weights of all particles are changed to remove the effects of the removed metric observations.
This method internally calls updateAreaFromLMH
void mrpt::hmtslam::CLocalMetricHypothesis::updateAreaFromLMH | ( | const CHMHMapNode::TNodeID | areaID, |
bool | eraseSFsFromLMH = false |
||
) |
The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are referenced to the area's reference poseID, such as that reference is at the origin.
If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied to the area, and removed from the LMH.
friend class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER [friend] |
Definition at line 96 of file CLocalMetricHypothesis.h.
mrpt::utils::CLASSINIT mrpt::hmtslam::CLocalMetricHypothesis::_init_CLocalMetricHypothesis [static, protected] |
Definition at line 99 of file CLocalMetricHypothesis.h.
mrpt::utils::TRuntimeClassId mrpt::hmtslam::CLocalMetricHypothesis::classCLocalMetricHypothesis [static] |
Definition at line 99 of file CLocalMetricHypothesis.h.
Definition at line 99 of file CLocalMetricHypothesis.h.
Used in CLSLAM_RBPF_2DLASER.
Definition at line 125 of file CLocalMetricHypothesis.h.
Used in CLSLAM_RBPF_2DLASER.
Definition at line 126 of file CLocalMetricHypothesis.h.
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to search for potential loop-closures. Set in CHMTSLAM::LSLAM_process_message_from_AA, read in.
Definition at line 119 of file CLocalMetricHypothesis.h.
The current robot pose (its global unique ID) for this hypothesis.
Definition at line 113 of file CLocalMetricHypothesis.h.
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
Definition at line 111 of file CLocalMetricHypothesis.h.
Critical section for threads signaling they are working with the LMH.
Definition at line 110 of file CLocalMetricHypothesis.h.
Log-weight of this hypothesis.
Definition at line 121 of file CLocalMetricHypothesis.h.
std::vector<std::map<TPoseID,double> > mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history |
The historic log-weights of the metric observations inserted in this LMH, for each particle.
Definition at line 122 of file CLocalMetricHypothesis.h.
std::vector<double> mrpt::hmtslam::CLocalMetricHypothesis::m_maxLikelihood [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 243 of file CLocalMetricHypothesis.h.
StdVector_CPose2D mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawMaximumLikelihood [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 255 of file CLocalMetricHypothesis.h.
std::vector<CPose2D,Eigen::aligned_allocator<CPose2D> > mrpt::hmtslam::CLocalMetricHypothesis::m_movementDraws [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 247 of file CLocalMetricHypothesis.h.
unsigned int mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawsIdx [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 251 of file CLocalMetricHypothesis.h.
The list of all areas sourronding the current one (this includes the current area itself).
Definition at line 115 of file CLocalMetricHypothesis.h.
The hybrid map node membership for each robot pose.
Definition at line 116 of file CLocalMetricHypothesis.h.
For quick access to our parent object.
Definition at line 112 of file CLocalMetricHypothesis.h.
vector_double mrpt::hmtslam::CLocalMetricHypothesis::m_pfAuxiliaryPFOptimal_estimatedProb [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 239 of file CLocalMetricHypothesis.h.
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop.
Definition at line 118 of file CLocalMetricHypothesis.h.
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning mrpt::hmtslam::CLocalMetricHypothesis::m_robotPosesGraph |
The SF gathered at each robot pose.
Definition at line 117 of file CLocalMetricHypothesis.h.
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |