42 : m_parent(parent), m_log_w_metric_history()
46 m_accumRobotMovementIsValid =
false;
52 CLocalMetricHypothesis::~CLocalMetricHypothesis() { clearParticles(); }
60 void CLocalMetricHypothesis::getAs3DScene(
70 std::make_shared<opengl::CGridPlaneXY>(-100, 100, -100, 100, 0, 5);
71 obj->setColor(0.4, 0.4, 0.4);
80 std::map<TPoseID, CPose3DPDFParticles> lstPoses;
81 std::map<TPoseID, CPose3DPDFParticles>::iterator it;
82 getPathParticles(lstPoses);
88 std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
90 for (
unsigned long m_neighbor : m_neighbors)
93 m_parent->m_map.getNodeByID(m_neighbor);
97 if (node->m_annotations.getElemental(
100 lstPoses[poseID_origin].getMean(originPose);
103 corner->setPose(originPose);
104 objs->insert(corner);
111 const CPose3D meanCurPose = lstPoses[m_currentRobotPose].getMeanVal();
114 cam->setZoomDistance(85);
115 cam->setAzimuthDegrees(45 +
RAD2DEG(meanCurPose.
yaw()));
116 cam->setElevationDegrees(45);
117 cam->setPointingAt(meanCurPose);
121 map<CHMHMapNode::TNodeID, CPoint3D>
123 map<CHMHMapNode::TNodeID, int> areas_howmany;
125 for (it = lstPoses.begin(); it != lstPoses.end(); it++)
129 if (m_nodeIDmemberships.find(it->first)->second ==
130 m_nodeIDmemberships.find(m_currentRobotPose)->second)
131 ellip->setColor(0, 0, 1);
133 ellip->setColor(1, 0, 0);
142 pdf.
mean.z() + 0.005);
150 pdf.
cov(2, 2) < 1e-4f)
153 ellip->setCovMatrix(C);
155 ellip->enableDrawSolid3D(
false);
158 ellip->setName(
format(
"P%u", (
unsigned int)it->first));
159 ellip->enableShowName();
167 0, 0, 0, 0.20f, 0, 0, 0.25f, 0.005f, 0.02f);
168 obj->setColor(1, 0, 0);
174 if (it->first == m_currentRobotPose)
178 robot->setPose(pdf.
mean);
186 auto itAreaId = m_nodeIDmemberships.find(it->first);
187 ASSERT_(itAreaId != m_nodeIDmemberships.end());
189 areas_howmany[areaId]++;
190 areas_mean[areaId].x_incr(pdf.
mean.
x());
191 areas_mean[areaId].y_incr(pdf.
mean.
y());
192 areas_mean[areaId].z_incr(pdf.
mean.z());
197 map<CHMHMapNode::TNodeID, CPoint3D>::iterator itMeans;
198 map<CHMHMapNode::TNodeID, int>::iterator itHowMany;
199 ASSERT_(areas_howmany.size() == areas_mean.size());
200 for (itMeans = areas_mean.begin(), itHowMany = areas_howmany.begin();
201 itMeans != areas_mean.end(); itMeans++, itHowMany++)
203 ASSERT_(itHowMany->second > 0);
205 float f = 1.0f / itHowMany->second;
206 itMeans->second *= f;
212 for (it = lstPoses.begin(); it != lstPoses.end(); it++)
214 if (it->first != m_currentRobotPose)
217 areas_mean[m_nodeIDmemberships.find(it->first)->second]);
218 areaPnt.z_incr(m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
225 std::make_shared<opengl::CSimpleLine>();
226 line->setColor(0.8, 0.8, 0.8, 0.3);
227 line->setLineWidth(2);
231 areaPnt.y(), areaPnt.z());
284 std::lock_guard<std::mutex> lock(
287 for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
292 if (itMeans->first ==
293 m_nodeIDmemberships.find(m_currentRobotPose)->second)
295 sphere->setColor(0.1, 0.1, 0.7);
299 sphere->setColor(0.7, 0, 0);
303 itMeans->second.x(), itMeans->second.y(),
304 itMeans->second.z() +
305 m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
307 sphere->setRadius(m_parent->m_options.VIEW3D_AREA_SPHERES_RADIUS);
310 objs->insert(sphere);
314 txt->setColor(1, 1, 1);
317 m_parent->m_map.getNodeByID(itMeans->first);
321 itMeans->second.x(), itMeans->second.y(),
322 itMeans->second.z() +
323 m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
326 txt->setString(
format(
"%li", (
long int)node->getID()));
336 std::lock_guard<std::mutex> lock(m_parent->m_map_cs);
338 for (itMeans = areas_mean.begin(); itMeans != areas_mean.end();
343 m_parent->m_map.getNodeByID(srcAreaID);
347 srcArea->getArcs(lstArcs);
348 for (
auto a = lstArcs.begin();
a != lstArcs.end(); ++
a)
351 if ((*a)->getNodeFrom() == srcAreaID)
353 auto trgAreaPoseIt = areas_mean.find((*a)->getNodeTo());
354 if (trgAreaPoseIt != areas_mean.end())
358 std::make_shared<opengl::CSimpleLine>();
359 line->setColor(0.8, 0.8, 0);
360 line->setLineWidth(3);
363 areas_mean[srcAreaID].
x(),
364 areas_mean[srcAreaID].
y(),
365 areas_mean[srcAreaID].
z() +
366 m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT,
367 trgAreaPoseIt->second.x(),
368 trgAreaPoseIt->second.y(),
369 trgAreaPoseIt->second.z() +
370 m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
382 void CLocalMetricHypothesis::prediction_and_update_pfAuxiliaryPFOptimal(
388 ASSERT_(m_parent->m_LSLAM_method);
389 m_parent->m_LSLAM_method->prediction_and_update_pfAuxiliaryPFOptimal(
390 this, action, observation, PF_options);
393 void CLocalMetricHypothesis::prediction_and_update_pfOptimalProposal(
399 ASSERT_(m_parent->m_LSLAM_method);
400 m_parent->m_LSLAM_method->prediction_and_update_pfOptimalProposal(
401 this, action, observation, PF_options);
416 std::map<TPoseID, CPose3DPDFParticles> parts;
417 getPathParticles(parts);
419 std::map<TPoseID, CPose3DPDFParticles>::iterator it;
421 for (it = parts.begin(); it != parts.end(); it++)
422 it->second.getMean(outList[it->first]);
430 void CLocalMetricHypothesis::getPathParticles(
431 std::map<TPoseID, CPose3DPDFParticles>& outList)
const 437 if (m_particles.empty())
return;
440 for (
auto itPoseID = m_particles.begin()->d->robotPoses.begin();
441 itPoseID != m_particles.begin()->d->robotPoses.end(); ++itPoseID)
444 CParticleList::const_iterator it;
445 CPose3DPDFParticles::CParticleList::iterator itP;
446 for (it = m_particles.begin(), itP = auxPDF.m_particles.begin();
447 it != m_particles.end(); it++, itP++)
449 itP->log_w = it->log_w;
450 itP->d = it->d->robotPoses.find(itPoseID->first)->second.asTPose();
454 outList[itPoseID->first] = auxPDF;
463 void CLocalMetricHypothesis::getPoseParticles(
470 CParticleList::const_iterator it;
472 CPose3DPDFParticles::CParticleList::iterator itP;
473 for (it = m_particles.begin(), itP = outPDF.
m_particles.begin();
474 it != m_particles.end(); it++, itP++)
476 itP->log_w = it->log_w;
477 auto itPose = it->d->robotPoses.find(poseID);
478 ASSERT_(itPose != it->d->robotPoses.end());
479 itP->d = itPose->second.asTPose();
488 void CLocalMetricHypothesis::clearRobotPoses()
491 m_particles.resize(m_parent->m_options.pf_options.sampleSize);
492 for (
auto& m_particle : m_particles)
495 m_particle.log_w = 0;
497 &m_parent->m_options.defaultMapsInitializers));
500 m_particle.d->robotPoses.clear();
507 const CPose3D* CLocalMetricHypothesis::getCurrentPose(
508 const size_t& particleIdx)
const 510 if (particleIdx >= m_particles.size())
513 auto it = m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
514 ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
521 CPose3D* CLocalMetricHypothesis::getCurrentPose(
const size_t& particleIdx)
523 if (particleIdx >= m_particles.size())
526 auto it = m_particles[particleIdx].d->robotPoses.find(m_currentRobotPose);
527 ASSERT_(it != m_particles[particleIdx].d->robotPoses.end());
534 void CLocalMetricHypothesis::getRelativePose(
543 CParticleList::const_iterator it;
544 CPose3DPDFParticles::CParticleList::iterator itP;
545 for (it = m_particles.begin(), itP = outPDF.
m_particles.begin();
546 it != m_particles.end(); it++, itP++)
548 itP->log_w = it->log_w;
550 auto srcPose = it->d->robotPoses.find(reference);
551 auto trgPose = it->d->robotPoses.find(pose);
553 ASSERT_(srcPose != it->d->robotPoses.end());
554 ASSERT_(trgPose != it->d->robotPoses.end());
565 void CLocalMetricHypothesis::changeCoordinateOrigin(
const TPoseID& newOrigin)
569 CParticleList::iterator it;
570 CPose3DPDFParticles::CParticleList::iterator itOrgPDF;
572 for (it = m_particles.begin(), itOrgPDF = originPDF.m_particles.begin();
573 it != m_particles.end(); it++, itOrgPDF++)
575 auto refPoseIt = it->d->robotPoses.find(newOrigin);
576 ASSERT_(refPoseIt != it->d->robotPoses.end());
577 const CPose3D& refPose = refPoseIt->second;
580 itOrgPDF->d = refPose.
asTPose();
581 itOrgPDF->log_w = it->log_w;
583 auto End = it->d->robotPoses.end();
585 for (
auto itP = it->d->robotPoses.begin(); itP != End; ++itP)
586 if (itP != refPoseIt) itP->second = itP->second - refPose;
589 refPoseIt->second.setFromValues(0, 0, 0);
597 std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
599 CSimpleMap* SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
600 for (
auto&
p : m_robotPosesGraph.idx2pose)
604 SFseq->
get(
p.first, pdf, sf);
609 getPoseParticles(
p.second, *pdfParts);
617 void CLocalMetricHypothesis::rebuildMetricMaps()
619 for (
auto& m_particle : m_particles)
621 m_particle.d->metricMaps.clear();
624 auto End = m_particle.d->robotPoses.end();
625 for (
auto itP = m_particle.d->robotPoses.begin(); itP != End; ++itP)
630 auto SFit = m_SFs.find(itP->first);
632 SFit->second.insertObservationsInto(
633 &m_particle.d->metricMaps, &itP->second);
651 void CLocalMetricHypothesis::removeAreaFromLMH(
658 auto itNeig = m_neighbors.find(areaID);
659 if (itNeig != m_neighbors.end()) m_neighbors.erase(itNeig);
664 for (
auto& m_nodeIDmembership : m_nodeIDmemberships)
665 if (m_nodeIDmembership.second == areaID)
666 lstPoseIDs.
insert(m_nodeIDmembership.first);
674 updateAreaFromLMH(areaID,
true);
679 for (
auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
680 for (
auto& m_particle : m_particles)
681 m_particle.d->robotPoses.erase(m_particle.d->robotPoses.find(*it));
687 CParticleList::iterator
p;
688 vector<map<TPoseID, double>>::iterator ws_it;
689 ASSERT_(m_log_w_metric_history.size() == m_particles.size());
691 for (ws_it = m_log_w_metric_history.begin(),
p = m_particles.begin();
692 p != m_particles.end(); ++
p, ++ws_it)
694 for (
auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
696 auto itW = ws_it->find(*it);
697 if (itW != ws_it->end())
701 p->log_w -= itW->second;
713 std::lock_guard<std::mutex> locker(m_robotPosesGraph.lock);
715 std::vector<uint32_t> indexesToRemove;
716 indexesToRemove.reserve(lstPoseIDs.size());
718 for (
auto it = m_robotPosesGraph.idx2pose.begin();
719 it != m_robotPosesGraph.idx2pose.end();)
721 if (lstPoseIDs.
find(it->second) != lstPoseIDs.end())
723 indexesToRemove.push_back(it->first);
729 m_robotPosesGraph.idx2pose.erase(it);
736 m_robotPosesGraph.partitioner.removeSetOfNodes(indexesToRemove);
741 map<uint32_t, TPoseID> newList;
742 for (
auto i = m_robotPosesGraph.idx2pose.begin();
743 i != m_robotPosesGraph.idx2pose.end(); ++i, idx++)
744 newList[idx] = i->second;
745 m_robotPosesGraph.idx2pose = newList;
756 for (
auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
757 m_nodeIDmemberships.erase(m_nodeIDmemberships.find(*it));
759 double out_max_log_w;
760 normalizeWeights(&out_max_log_w);
768 unsigned int CLocalMetricHypothesis::TRobotPosesPartitioning::pose2idx(
771 for (
auto it : idx2pose)
772 if (it.second ==
id)
return it.first;
782 void CLocalMetricHypothesis::updateAreaFromLMH(
788 for (
auto it = m_nodeIDmemberships.begin(); it != m_nodeIDmemberships.end();
790 if (it->second == areaID) lstPoseIDs.
insert(it->first);
796 std::lock_guard<std::mutex>(m_parent->m_map_cs);
797 node = m_parent->m_map.getNodeByID(areaID);
799 ASSERT_(node->m_hypotheses.has(m_ID));
804 node->m_annotations.getElemental(
812 CSerializable::Ptr annot =
817 posesGraph = std::make_shared<CRobotPosesGraph>();
818 node->m_annotations.setMemoryReference(
830 bool pdfOrigin_ok =
false;
831 for (
auto it = lstPoseIDs.begin(); it != lstPoseIDs.end(); ++it)
833 TPoseInfo& poseInfo = (*posesGraph)[*it];
834 getPoseParticles(*it, poseInfo.
pdf);
837 if (*it == poseID_origin)
843 if (*it != m_currentRobotPose)
845 auto itSF = m_SFs.find(*it);
850 poseInfo.
sf = std::move(itSF->second);
855 poseInfo.
sf = itSF->second;
863 pdfOrigin.
inverse(pdfOriginInv);
864 for (
auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
866 CPose3DPDFParticles::CParticleList::iterator orgIt, pdfIt;
867 ASSERT_(it->second.pdf.size() == pdfOriginInv.
size());
868 for (pdfIt = it->second.pdf.m_particles.begin(),
870 orgIt != pdfOriginInv.
m_particles.end(); orgIt++, pdfIt++)
880 posesGraph->insertIntoMetricMap(*metricMap);
886 void CLocalMetricHypothesis::dumpAsText(std::vector<std::string>& st)
const 889 st.emplace_back(
"LIST OF POSES IN LMH");
890 st.emplace_back(
"====================");
894 for (
unsigned long m_neighbor : m_neighbors)
895 s +=
format(
"%i ", (
int)m_neighbor);
901 for (
auto it = lst.begin(); it != lst.end(); ++it)
903 auto area = m_nodeIDmemberships.find(it->first);
905 " ID: %i \t AREA: %i \t %.03f,%.03f,%.03fdeg", (
int)it->first,
906 (
int)area->second, it->second.x(), it->second.y(),
914 void CLocalMetricHypothesis::serializeFrom(
921 in >> m_ID >> m_currentRobotPose >> m_neighbors >>
922 m_nodeIDmemberships >> m_SFs >> m_posesPendingAddPartitioner >>
923 m_areasPendingTBI >> m_log_w >> m_log_w_metric_history >>
924 m_robotPosesGraph.partitioner >> m_robotPosesGraph.idx2pose;
927 readParticlesFromStream(
in);
935 uint8_t CLocalMetricHypothesis::serializeGetVersion()
const {
return 0; }
936 void CLocalMetricHypothesis::serializeTo(
939 out << m_ID << m_currentRobotPose << m_neighbors << m_nodeIDmemberships
940 << m_SFs << m_posesPendingAddPartitioner << m_areasPendingTBI << m_log_w
941 << m_log_w_metric_history << m_robotPosesGraph.partitioner
942 << m_robotPosesGraph.idx2pose;
945 writeParticlesToStream(out);
948 void CLSLAMParticleData::serializeFrom(
955 in >> metricMaps >> robotPoses;
963 uint8_t CLSLAMParticleData::serializeGetVersion()
const {
return 0; }
966 out << metricMaps << robotPoses;
mrpt::math::TPose3D asTPose() const
CPose3D mean
The mean value.
double RAD2DEG(const double x)
Radians to degrees.
std::list< T >::iterator find(const T &i)
#define THROW_EXCEPTION(msg)
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
CParticleList m_particles
The array of particles.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
GLsizei GLsizei GLuint * obj
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Declares a class for storing a collection of robot actions.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define ASSERT_(f)
Defines an assertion mechanism.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
static Ptr Create(Args &&... args)
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
double x() const
Common members of all points & poses classes.
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i'th pair, first one is index '0'.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
#define NODE_ANNOTATION_METRIC_MAPS
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
CSetOfObjects::Ptr RobotPioneer()
Returns a representation of a Pioneer II mobile base.
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
constexpr std::size_t size() const
Virtual base class for "archives": classes abstracting I/O streams.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
The configuration of a particle filter.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
mrpt::obs::CSensoryFrame sf
The observations.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
The namespace for 3D scene representation and rendering.
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Information kept for each robot pose used in CRobotPosesGraph.
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
This class stores any customizable set of metric maps.
#define NODE_ANNOTATION_POSES_GRAPH
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
GLubyte GLubyte GLubyte a
A class for storing a sequence of arcs (a path).
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).