44 return kf1.metric_map->compute3DMatchingRatio(kf2.metric_map.get(), relPose2wrt1, parent->options.mrp);
58 metricmap.push_back(def);
72 "minDistForCorrespondence",
double, mrp.maxDistForCorr,
75 "minMahaDistForCorrespondence",
double, mrp.maxMahaDistForCorr,
80 metricmap.loadFromConfigFile(cfp,
"metricmap");
81 MRPT_TODO(
"Add link to example INI file");
91 MRPT_SAVE_CONFIG_VAR_COMMENT(forceBisectionOnly,
"Force bisection (true) or automatically determine number of partitions(false = default)");
99 metricmap.saveToConfigFile(cfp,
"metricmap");
104 m_last_last_partition_are_new_ones =
false;
106 m_individualFrames.clear();
107 m_individualMaps.clear();
108 m_last_partition.clear();
117 const uint32_t new_id = m_individualMaps.size();
118 const size_t n = new_id + 1;
121 m_individualMaps.push_back(CMultiMetricMap::Create());
122 auto& newMetricMap = m_individualMaps.back();
123 newMetricMap->setListOfMaps(&options.metricmap);
129 m_individualFrames.insert(&robotPose, frame);
134 ASSERT_(m_individualMaps.size() ==
n);
135 ASSERT_(m_individualFrames.size() ==
n);
139 using namespace std::placeholders;
140 switch (options.simil_method)
149 sim_func = m_sim_func;
167 auto pose_i = posePDF_i->getMeanVal();
169 for (
uint32_t j = 0; j < new_id; j++)
171 const auto id_diff = new_id - j;
173 if (id_diff > options.maxKeyFrameDistanceToEval)
185 auto pose_j = posePDF_j->getMeanVal();
188 auto relPose = pose_j - pose_i;
191 const auto s_ij = sim_func(map_i, map_j, relPose);
192 const auto s_ji = sim_func(map_j, map_i, relPose);
193 s_sym = 0.5*(s_ij + s_ji);
195 m_A(i, j) = m_A(j, i) = s_sym;
200 m_A(new_id, new_id) = 0;
206 if (m_last_last_partition_are_new_ones)
209 m_last_partition[m_last_partition.size() - 1].push_back(
n - 1);
214 std::vector<uint32_t> dummyPart;
215 dummyPart.push_back(
n - 1);
216 m_last_partition.emplace_back(dummyPart);
219 m_last_last_partition_are_new_ones =
true;
228 vector<std::vector<uint32_t>>& partitions)
234 m_A, partitions, options.partitionThreshold,
true,
true,
235 !options.forceBisectionOnly,
236 options.minimumNumberElementsEachCluster,
false 239 m_last_partition = partitions;
240 m_last_last_partition_are_new_ones =
false;
247 return m_individualFrames.size();
251 std::vector<uint32_t> indexesToRemove,
bool changeCoordsRef)
255 size_t nOld = m_A.cols();
256 size_t nNew = nOld - indexesToRemove.size();
260 std::sort(indexesToRemove.begin(), indexesToRemove.end());
265 std::vector<uint32_t> indexesToStay;
266 indexesToStay.reserve(nNew);
267 for (i = 0; i < nOld; i++)
270 for (j = 0; !remov && j < indexesToRemove.size(); j++)
272 if (indexesToRemove[j] == i) remov =
true;
275 if (!remov) indexesToStay.push_back(i);
278 ASSERT_(indexesToStay.size() == nNew);
283 for (i = 0; i < nNew; i++)
284 for (j = 0; j < nNew; j++)
285 newA(i, j) = m_A(indexesToStay[i], indexesToStay[j]);
292 m_last_partition.resize(1);
293 m_last_partition[0].resize(nNew);
294 for (i = 0; i < nNew; i++) m_last_partition[0][i] = i;
296 m_last_last_partition_are_new_ones =
false;
300 for (
auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
302 auto itM = m_individualMaps.begin() + *it;
303 m_individualMaps.erase(itM);
308 for (
auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
309 m_individualFrames.remove(*it);
317 ASSERT_(m_individualFrames.size() > 0);
318 m_individualFrames.get(0, posePDF, SF);
322 m_individualFrames.changeCoordinatesOrigin(
p);
332 m_individualFrames.changeCoordinatesOrigin(newOrigin);
336 unsigned int newOriginPose)
342 m_individualFrames.get(newOriginPose, pdf, sf);
346 changeCoordinatesOrigin(-
p);
353 const std::map<uint32_t, int64_t>* renameIndexes)
const 356 ASSERT_((
int)m_individualFrames.size() == m_A.cols());
359 objs->insert(gl_grid);
360 int bbminx = std::numeric_limits<int>::max(), bbminy = std::numeric_limits<int>::max();
361 int bbmaxx = -bbminx, bbmaxy = -bbminy;
363 for (
size_t i = 0; i < m_individualFrames.size(); i++)
367 m_individualFrames.get(i, i_pdf, i_sf);
370 i_pdf->getMean(i_mean);
378 mrpt::make_aligned_shared<opengl::CSphere>();
379 i_sph->setRadius(0.02f);
380 i_sph->setColor(0, 0, 1);
383 i_sph->setName(
format(
"%u", static_cast<unsigned int>(i)));
387 renameIndexes->find(i);
388 ASSERT_(itName != renameIndexes->end());
390 format(
"%lu", static_cast<unsigned long>(itName->second)));
393 i_sph->enableShowName();
394 i_sph->setPose(i_mean);
399 for (
size_t j = i + 1; j < m_individualFrames.size(); j++)
403 m_individualFrames.get(j, j_pdf, j_sf);
406 j_pdf->getMean(j_mean);
408 float SSO_ij = m_A(i, j);
413 mrpt::make_aligned_shared<opengl::CSimpleLine>();
415 i_mean.
x(), i_mean.
y(), i_mean.z(), j_mean.
x(), j_mean.
y(),
418 lin->setColor(SSO_ij, 0, 1 - SSO_ij, SSO_ij * 0.6);
419 lin->setLineWidth(SSO_ij * 10);
425 gl_grid->setPlaneLimits(bbminx, bbmaxx, bbminy, bbmaxy);
426 gl_grid->setGridFrequency(5);
437 in >> m_individualFrames >> m_individualMaps >> m_A >>
438 m_last_partition >> m_last_last_partition_are_new_ones;
442 std::vector<uint8_t> old_modified_nodes;
443 in >> old_modified_nodes;
456 out << m_individualFrames << m_individualMaps << m_A << m_last_partition
457 << m_last_last_partition_are_new_ones;
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
double x() const
Common members of all points & poses classes.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
size_t getNodesCount()
Get the total node count currently in the internal map/graph.
std::function< double(const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)> similarity_func_t
Type of similarity evaluator for map keyframes.
void removeSetOfNodes(std::vector< uint32_t > indexesToRemove, bool changeCoordsRef=true)
Remove a list of keyframes, with indices as returned by addMapFrame()
#define THROW_EXCEPTION(msg)
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::maps::CMultiMetricMap::Ptr metric_map
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
int MRPT_SAVE_NAME_PADDING()
Default padding sizes for macros MRPT_SAVE_CONFIG_VAR_COMMENT(), etc.
Abstract graph and tree data structures, plus generic graph algorithms.
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...
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
static double eval_similarity_metric_map_matching(const CIncrementalMapPartitioner *parent, const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)
#define ASSERT_(f)
Defines an assertion mechanism.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
void changeCoordinatesOriginPoseIndex(unsigned int newOriginPose)
The new origin is given by the index of the pose that is to become the new origin.
Map keyframe, comprising raw observations and they as a metric map.
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...
void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string §ion) const
This method saves the options to a ".ini"-like file or memory-stored string list. ...
mrpt::obs::CSensoryFrame::Ptr raw_observations
int MRPT_SAVE_VALUE_PADDING()
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs, const std::map< uint32_t, int64_t > *renameIndexes=NULL) const
Return a 3D representation of the graph: poses & links between them.
A wrapper for other CConfigFileBase-based objects that prefixes a given token to every key and/or sec...
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#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...
unsigned __int64 uint64_t
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static double eval_similarity_observation_overlap(const map_keyframe_t &kf1, const map_keyframe_t &kf2, const mrpt::poses::CPose3D &relPose2wrt1)
Finds partitions in metric maps based on N-cut graph partition theory.
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).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei GLsizei GLchar * source
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
unsigned __int32 uint32_t
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=nullptr) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
Algorithms for finding the min-normalized-cut of a weighted undirected graph.
static Ptr Create(Args &&... args)
const Scalar * const_iterator
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...