47 m_last_last_partition_are_new_ones(false),
61 : partitionThreshold(1.0f),
62 gridResolution(0.10f),
63 minDistForCorrespondence(0.20f),
64 minMahaDistForCorrespondence(2.0f),
65 forceBisectionOnly(false),
67 minimumNumberElementsEachCluster(1)
85 minimumNumberElementsEachCluster,
int,
source, section);
97 "\n----------- [CIncrementalMapPartitioner::TOptions] ------------ " 101 "partitionThreshold = %f\n", partitionThreshold);
103 "gridResolution = %f\n", gridResolution);
105 "minDistForCorrespondence = %f\n",
106 minDistForCorrespondence);
108 "forceBisectionOnly = %c\n",
109 forceBisectionOnly ?
'Y' :
'N');
111 "useMapMatching = %c\n",
112 useMapMatching ?
'Y' :
'N');
114 "minimumNumberElementsEachCluster = %i\n",
115 minimumNumberElementsEachCluster);
157 size_t i = 0, j = 0,
n = 0;
158 CPose3D pose_i, pose_j, relPose;
163 static CPose3D nullPose(0, 0, 0);
189 newMetricMap.
m_pointsMaps[0]->insertionOptions.isPlanarMap =
false;
190 newMetricMap.
m_pointsMaps[0]->insertionOptions.minDistBetweenLaserPoints =
196 ->insertionOptions.minDistBetweenLaserPoints);
216 frame->insertObservationsInto(&lm);
224 n =
m_A.getColCount();
250 posePDF_i->getMean(pose_i);
255 for (j = 0; j <
n - 1; j++)
259 posePDF_j->getMean(pose_j);
261 relPose = pose_j - pose_i;
281 for (i = 0; i <
n - 1;
286 posePDF_i->getMean(pose_i);
295 posePDF_j->getMean(pose_j);
297 relPose = pose_j - pose_i;
317 m_A(
n - 1,
n - 1) = 0;
321 for (i = 0; i <
n; i++)
322 for (j = i + 1; j <
n; j++)
342 dummyPart.push_back(
n - 1);
352 cout <<
"Unexpected runtime error:\n"; cout <<
"\tn=" <<
n <<
"\n";
353 cout <<
"\ti=" << i <<
"\n"; cout <<
"\tj=" << j <<
"\n";
354 cout <<
"\tmap_i=" << map_i <<
"\n";
355 cout <<
"\tmap_j=" << map_j <<
"\n";
356 cout <<
"relPose: " << relPose << endl;
357 cout <<
"map_i.size()=" << map_i->m_pointsMaps[0]->
size() <<
"\n";
358 cout <<
"map_j.size()=" << map_j->m_pointsMaps[0]->size() <<
"\n";
359 map_i->m_pointsMaps[0]->save2D_to_text_file(
360 string(
"debug_DUMP_map_i.txt"));
361 map_j->m_pointsMaps[0]->save2D_to_text_file(
362 string(
"debug_DUMP_map_j.txt"));
363 m_A.saveToTextFile(
"debug_DUMP_exception_A.txt"););
370 vector<vector_uint>& partitions)
375 unsigned int n_nodes;
376 unsigned int n_clusters_last;
384 last_parts_are_mods.resize(n_clusters_last);
389 for (i = 0; i < n_clusters_last; i++)
394 last_parts_are_mods[i] =
false;
400 last_parts_are_mods[i] =
true;
403 if (last_parts_are_mods[i])
409 for (i = 0; i < n_nodes; i++)
420 A_mods.setSize(mods.size(), mods.size());
421 for (i = 0; i < mods.size(); i++)
423 for (j = 0; j < mods.size(); j++)
425 A_mods(i, j) =
m_A(mods[i], mods[j]);
430 vector<vector_uint> mods_parts;
447 if (!last_parts_are_mods[i])
453 for (i = 0; i < mods_parts.size(); i++)
457 for (j = 0; j < mods_parts[i].size(); j++)
458 v.push_back(mods[mods_parts[i][j]]);
460 partitions.push_back(
v);
469 size_t n = partitions.size();
494 size_t nOld =
m_A.getColCount();
495 size_t nNew = nOld - indexesToRemove.size();
499 std::sort(indexesToRemove.begin(), indexesToRemove.end());
505 indexesToStay.reserve(nNew);
506 for (i = 0; i < nOld; i++)
509 for (j = 0; !remov && j < indexesToRemove.size(); j++)
511 if (indexesToRemove[j] == i) remov =
true;
514 if (!remov) indexesToStay.push_back(i);
517 ASSERT_(indexesToStay.size() == nNew);
522 for (i = 0; i < nNew; i++)
523 for (j = 0; j < nNew; j++)
524 newA(i, j) =
m_A(indexesToStay[i], indexesToStay[j]);
542 vector_uint::reverse_iterator it;
543 for (it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
553 for (it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
601 const unsigned& newOriginPose)
621 const std::map<uint32_t, int64_t>* renameIndexes)
const 627 mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
628 -100, 100, -100, 100, 0, 5));
637 i_pdf->getMean(i_mean);
640 mrpt::make_aligned_shared<opengl::CSphere>();
641 i_sph->setRadius(0.02f);
642 i_sph->setColor(0, 0, 1);
645 i_sph->setName(
format(
"%u", static_cast<unsigned int>(i)));
649 renameIndexes->find(i);
650 ASSERT_(itName != renameIndexes->end());
652 format(
"%lu", static_cast<unsigned long>(itName->second)));
655 i_sph->enableShowName();
656 i_sph->setPose(i_mean);
668 j_pdf->getMean(j_mean);
670 float SSO_ij =
m_A(i, j);
675 mrpt::make_aligned_shared<opengl::CSimpleLine>();
677 i_mean.
x(), i_mean.
y(), i_mean.z(), j_mean.
x(), j_mean.
y(),
680 lin->setColor(SSO_ij, 0, 1 - SSO_ij, SSO_ij * 0.6);
681 lin->setLineWidth(SSO_ij * 10);
734 std::dynamic_pointer_cast<CPose3DPDF>(
float partitionThreshold
The partition threshold for bisection in range [0,2], default=1.0.
std::deque< mrpt::maps::CMultiMetricMap > m_individualMaps
double x() const
Common members of all points & poses classes.
Parameters for CMetricMap::compute3DMatchingRatio()
mrpt::maps::CSimpleMap m_individualFrames
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
std::vector< uint32_t > vector_uint
mrpt::math::CMatrixD m_A
Adjacency matrix.
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMap::Ptr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
#define MRPT_END_WITH_CLEAN_UP(stuff)
unsigned int getNodesCount()
Get the total node count currently in the internal map/graph.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
void markAllNodesForReconsideration()
Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering jus...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool useMapMatching
If set to true (default), adjacency matrix is computed from maps matching; otherwise, the method CObservation::likelihoodWith will be called directly from the SFs.
Abstract graph and tree data structures, plus generic graph algorithms.
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
double observationsOverlap(const mrpt::obs::CObservation *o1, const mrpt::obs::CObservation *o2, const mrpt::poses::CPose3D *pose_o2_wrt_o1=nullptr)
Estimates the "overlap" or "matching ratio" of two observations (range [0,1]), possibly taking into a...
const Scalar * const_iterator
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void removeSetOfNodes(vector_uint indexesToRemove, bool changeCoordsRef=true)
Remove the stated nodes (0-based indexes) from the internal lists.
void clear()
Clear the contents of this container.
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
This class allows loading and storing values and vectors of different types from a configuration text...
std::vector< bool > vector_bool
A type for passing a vector of bools.
float maxDistForCorr
(Default: 0.10f) The minimum distance between 2 non-probabilistic map elements for counting them as a...
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
void changeCoordinatesOriginPoseIndex(const unsigned &newOriginPose)
The new origin is given by the index of the pose that is to become the new origin.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This base provides a set of functions for maths stuff.
std::shared_ptr< CPosePDF > Ptr
A class for storing a map of 3D probabilistic landmarks.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const override
See the definition in the base class: Calls in this class become a call to every single map in this s...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
std::shared_ptr< CPose3DPDF > Ptr
std::shared_ptr< CSetOfObjects > Ptr
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::slam::CIncrementalMapPartitioner::TOptions options
std::shared_ptr< CSensoryFrame > Ptr
ProxySelectorContainerByClass< mrpt::maps::CLandmarksMap::Ptr, TListMaps > m_landmarksMap
Proxy that looks like a smart pointer to the first matching object in maps.
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs, const std::map< uint32_t, int64_t > *renameIndexes=NULL) const
Return a 3D representation of the current state: poses & links between them.
mrpt::utils::CObject::Ptr duplicateGetSmartPtr() const
Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object w...
size_t size() const
Returns the count of pairs (pose,sensory data)
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'.
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
void push_back(const MAP_DEFINITION &o)
std::vector< vector_uint > m_last_partition
The last partition.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
int minimumNumberElementsEachCluster
If a partition leads to a cluster with less elements than this, it will be rejected even if had a goo...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
virtual ~CIncrementalMapPartitioner()
dtor
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps' content...
float minDistForCorrespondence
bool m_last_last_partition_are_new_ones
This will be true after adding new observations, and before an "updatePartitions" is invoked...
GLsizei GLsizei GLchar * source
float maxMahaDistForCorr
(Default: 2.0f) The minimum Mahalanobis distance between 2 probabilistic map elements for counting th...
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
This class stores any customizable set of metric maps.
unsigned int addMapFrame(const mrpt::obs::CSensoryFrame::Ptr &frame, const mrpt::poses::CPosePDF::Ptr &robotPose2D)
float minMahaDistForCorrespondence
This class is a "CSerializable" wrapper for "CMatrixFloat".
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
Change the coordinate origin of all stored poses, for consistency with future new poses to enter in t...
std::shared_ptr< CSimpleLine > Ptr
std::shared_ptr< CSphere > Ptr
Algorithms for finding the min-normalized-cut of a weighted undirected graph.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
void clear()
Remove all stored pairs.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void remove(size_t index)
Deletes the i'th pair, first one is index '0'.
std::vector< uint8_t > m_modified_nodes
The list of keyframes to consider in the next update.