25 #include <Eigen/Dense>
42 return kf1.metric_map->compute3DMatchingRatio(
43 kf2.metric_map.get(), relPose2wrt1, parent->options.mrp);
56 metricmap.push_back(def);
68 minimumNumberElementsEachCluster, uint64_t, source, section);
70 "minDistForCorrespondence",
double, mrp.maxDistForCorr, source,
73 "minMahaDistForCorrespondence",
double, mrp.maxMahaDistForCorr, source,
78 source, section + std::string(
"."),
"");
79 metricmap.loadFromConfigFile(cfp,
"metricmap");
80 MRPT_TODO(
"Add link to example INI file");
89 partitionThreshold,
"N-cut partition threshold [0,2]");
92 "Force bisection (true) or automatically determine number of "
93 "partitions(false = default)");
97 maxKeyFrameDistanceToEval,
"Max KF ID distance");
99 s,
"minDistForCorrespondence", mrp.maxDistForCorr,
103 s,
"minMahaDistForCorrespondence", mrp.maxMahaDistForCorr,
108 metricmap.saveToConfigFile(cfp,
"metricmap");
113 m_last_last_partition_are_new_ones =
false;
115 m_individualFrames.clear();
116 m_individualMaps.clear();
117 m_last_partition.clear();
126 const uint32_t new_id = m_individualMaps.size();
127 const size_t n = new_id + 1;
130 m_individualMaps.push_back(CMultiMetricMap::Create());
131 auto& newMetricMap = m_individualMaps.back();
132 newMetricMap->setListOfMaps(options.metricmap);
138 m_individualFrames.insert(&robotPose, frame);
143 ASSERT_(m_individualMaps.size() == n);
144 ASSERT_(m_individualFrames.size() == n);
148 using namespace std::placeholders;
149 switch (options.simil_method)
152 sim_func = std::bind(
159 sim_func = m_sim_func;
177 auto pose_i = posePDF_i->getMeanVal();
179 for (uint32_t j = 0; j < new_id; j++)
181 const auto id_diff = new_id - j;
183 if (id_diff > options.maxKeyFrameDistanceToEval)
195 auto pose_j = posePDF_j->getMeanVal();
198 auto relPose = pose_j - pose_i;
201 const auto s_ij = sim_func(map_i, map_j, relPose);
202 const auto s_ji = sim_func(map_j, map_i, relPose);
203 s_sym = 0.5 * (s_ij + s_ji);
205 m_A(i, j) = m_A(j, i) = s_sym;
210 m_A(new_id, new_id) = 0;
216 if (m_last_last_partition_are_new_ones)
219 m_last_partition[m_last_partition.size() - 1].push_back(n - 1);
224 std::vector<uint32_t> dummyPart;
225 dummyPart.push_back(n - 1);
226 m_last_partition.emplace_back(dummyPart);
229 m_last_last_partition_are_new_ones =
true;
238 vector<std::vector<uint32_t>>& partitions)
244 m_A, partitions, options.partitionThreshold,
true,
true,
245 !options.forceBisectionOnly, options.minimumNumberElementsEachCluster,
249 m_last_partition = partitions;
250 m_last_last_partition_are_new_ones =
false;
257 return m_individualFrames.size();
261 std::vector<uint32_t> indexesToRemove,
bool changeCoordsRef)
265 size_t nOld = m_A.cols();
266 size_t nNew = nOld - indexesToRemove.size();
270 std::sort(indexesToRemove.begin(), indexesToRemove.end());
275 std::vector<uint32_t> indexesToStay;
276 indexesToStay.reserve(nNew);
277 for (i = 0; i < nOld; i++)
280 for (j = 0; !remov && j < indexesToRemove.size(); j++)
282 if (indexesToRemove[j] == i) remov =
true;
285 if (!remov) indexesToStay.push_back(i);
288 ASSERT_(indexesToStay.size() == nNew);
293 for (i = 0; i < nNew; i++)
294 for (j = 0; j < nNew; j++)
295 newA(i, j) = m_A(indexesToStay[i], indexesToStay[j]);
302 m_last_partition.
resize(1);
303 m_last_partition[0].resize(nNew);
304 for (i = 0; i < nNew; i++) m_last_partition[0][i] = i;
306 m_last_last_partition_are_new_ones =
false;
310 for (
auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
312 auto itM = m_individualMaps.begin() + *it;
313 m_individualMaps.erase(itM);
318 for (
auto it = indexesToRemove.rbegin(); it != indexesToRemove.rend(); ++it)
319 m_individualFrames.remove(*it);
327 ASSERT_(m_individualFrames.size() > 0);
328 m_individualFrames.get(0, posePDF, SF);
332 m_individualFrames.changeCoordinatesOrigin(p);
342 m_individualFrames.changeCoordinatesOrigin(newOrigin);
346 unsigned int newOriginPose)
352 m_individualFrames.get(newOriginPose, pdf, sf);
356 changeCoordinatesOrigin(-p);
363 const std::map<uint32_t, int64_t>* renameIndexes)
const
366 ASSERT_((
int)m_individualFrames.size() == m_A.cols());
369 objs->insert(gl_grid);
370 int bbminx = std::numeric_limits<int>::max(),
371 bbminy = std::numeric_limits<int>::max();
372 int bbmaxx = -bbminx, bbmaxy = -bbminy;
374 for (
size_t i = 0; i < m_individualFrames.size(); i++)
378 m_individualFrames.get(i, i_pdf, i_sf);
381 i_pdf->getMean(i_mean);
389 i_sph->setRadius(0.02f);
390 i_sph->setColor(0, 0, 1);
393 i_sph->setName(
format(
"%u",
static_cast<unsigned int>(i)));
396 auto itName = renameIndexes->find(i);
397 ASSERT_(itName != renameIndexes->end());
399 format(
"%lu",
static_cast<unsigned long>(itName->second)));
402 i_sph->enableShowName();
403 i_sph->setPose(i_mean);
408 for (
size_t j = i + 1; j < m_individualFrames.size(); j++)
412 m_individualFrames.get(j, j_pdf, j_sf);
415 j_pdf->getMean(j_mean);
417 float SSO_ij = m_A(i, j);
422 std::make_shared<opengl::CSimpleLine>();
424 i_mean.
x(), i_mean.
y(), i_mean.z(), j_mean.
x(), j_mean.
y(),
427 lin->setColor(SSO_ij, 0, 1 - SSO_ij, SSO_ij * 0.6);
428 lin->setLineWidth(SSO_ij * 10);
434 gl_grid->setPlaneLimits(bbminx, bbmaxx, bbminy, bbmaxy);
435 gl_grid->setGridFrequency(5);
446 in >> m_individualFrames >> m_individualMaps >> m_A >>
447 m_last_partition >> m_last_last_partition_are_new_ones;
451 std::vector<uint8_t> old_modified_nodes;
452 in >> old_modified_nodes;
465 out << m_individualFrames << m_individualMaps << m_A << m_last_partition
466 << m_last_last_partition_are_new_ones;