21 template <
class GRAPH_T>
24 this->initializeLoggers(
"CLoopCloserERD");
25 m_edge_types_to_nums[
"ICP2D"] = 0;
26 m_edge_types_to_nums[
"LC"] = 0;
30 template <
class GRAPH_T>
33 for (
auto it = m_node_optimal_paths.begin();
34 it != m_node_optimal_paths.end(); ++it)
40 template <
class GRAPH_T>
47 this->m_time_logger.enter(
"updateState");
59 getObservation<CObservation2DRangeScan>(observations, observation);
62 m_last_laser_scan2D = scan;
66 if (!m_first_laser_scan)
68 m_first_laser_scan = m_last_laser_scan2D;
72 size_t num_registered =
73 absDiff(this->m_last_total_num_nodes, this->m_graph->nodeCount());
74 bool registered_new_node = num_registered > 0;
76 if (registered_new_node)
79 registered_new_node =
true;
83 if (!this->m_override_registered_nodes_check)
85 if (!((num_registered == 1) ^
86 (num_registered == 2 && m_is_first_time_node_reg)))
89 "Invalid number of registered nodes since last call to "
92 << num_registered <<
"\" new nodes.");
103 if (m_is_first_time_node_reg)
106 "Assigning first laserScan to the graph root node.");
107 this->m_nodes_to_laser_scans2D[this->m_graph->root] =
109 m_is_first_time_node_reg =
false;
113 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
116 if (m_laser_params.use_scan_matching)
119 this->addScanMatchingEdges(this->m_graph->nodeCount() - 1);
123 m_partitions_full_update =
124 ((this->m_graph->nodeCount() %
125 m_lc_params.full_partition_per_nodes) == 0 ||
126 this->m_just_inserted_lc)
129 this->updateMapPartitions(
130 m_partitions_full_update, num_registered == 2);
134 this->checkPartitionsForLC(&partitions_for_LC);
135 this->evaluatePartitionsForLC(partitions_for_LC);
137 if (m_visualize_curr_node_covariance)
139 this->execDijkstraProjection();
142 this->m_last_total_num_nodes = this->m_graph->nodeCount();
145 this->m_time_logger.leave(
"updateState");
150 template <
class GRAPH_T>
153 std::set<mrpt::graphs::TNodeID>* nodes_set)
159 int fetched_nodeIDs = 0;
160 for (
int nodeID_i =
static_cast<int>(curr_nodeID) - 1;
161 ((fetched_nodeIDs <= this->m_laser_params.prev_nodes_for_ICP) &&
166 nodes_set->insert(nodeID_i);
171 template <
class GRAPH_T>
177 using namespace mrpt;
185 std::set<TNodeID> nodes_set;
186 this->fetchNodeIDsForScanMatching(curr_nodeID, &nodes_set);
189 for (
unsigned long node_it : nodes_set)
195 "Fetching laser scan for nodes: " << node_it <<
"==> "
199 this->getICPEdge(node_it, curr_nodeID, &rel_edge, &icp_info);
200 if (!found_edge)
continue;
207 m_laser_params.goodness_threshold_win.addNewMeasurement(
210 double goodness_thresh =
211 m_laser_params.goodness_threshold_win.getMedian() *
212 m_consec_icp_constraint_factor;
213 bool accept_goodness = icp_info.
goodness > goodness_thresh;
215 "Curr. Goodness: " << icp_info.
goodness
216 <<
"|\t Threshold: " << goodness_thresh <<
" => "
217 << (accept_goodness ?
"ACCEPT" :
"REJECT"));
221 bool accept_mahal_distance = this->mahalanobisDistanceOdometryToICPEdge(
222 node_it, curr_nodeID, rel_edge);
225 if (accept_goodness && accept_mahal_distance)
227 this->registerNewEdge(node_it, curr_nodeID, rel_edge);
233 template <
class GRAPH_T>
241 this->m_time_logger.enter(
"getICPEdge");
258 "TGetICPEdgeAdParams:\n"
265 bool from_success = this->getPropsOfNodeID(
266 from, &from_pose, from_scan, from_params);
269 bool to_success = this->getPropsOfNodeID(to, &to_pose, to_scan, to_params);
271 if (!from_success || !to_success)
275 << from <<
" or node #" << to
276 <<
" doesn't contain a valid LaserScan. Ignoring this...");
289 initial_estim = to_pose - from_pose;
293 "from_pose: " << from_pose <<
"| to_pose: " << to_pose
294 <<
"| init_estim: " << initial_estim);
296 range_ops_t::getICPEdge(
297 *from_scan, *to_scan, rel_edge, &initial_estim, icp_info);
300 this->m_time_logger.leave(
"getICPEdge");
305 template <
class GRAPH_T>
308 const std::map<mrpt::graphs::TNodeID, node_props_t>& group_params,
319 auto search = group_params.find(nodeID);
321 if (search == group_params.end())
327 *node_props = search->second;
337 template <
class GRAPH_T>
346 bool filled_pose =
false;
347 bool filled_scan =
false;
354 *pose = node_props->
pose;
358 if (node_props->
scan)
360 scan = node_props->
scan;
367 !(filled_pose ^ filled_scan),
369 "Either BOTH or NONE of the filled_pose, filled_scan should be set."
371 static_cast<unsigned long>(nodeID)));
379 auto search = this->m_graph->nodes.find(nodeID);
380 if (search != this->m_graph->nodes.end())
382 *pose = search->second;
392 auto search = this->m_nodes_to_laser_scans2D.find(nodeID);
393 if (search != this->m_nodes_to_laser_scans2D.end())
395 scan = search->second;
400 return filled_pose && filled_scan;
403 template <
class GRAPH_T>
408 this->m_time_logger.enter(
"LoopClosureEvaluation");
411 using namespace mrpt;
414 partitions_for_LC->clear();
418 map<int, std::vector<uint32_t>>::iterator finder;
420 if (m_partitions_full_update)
422 m_partitionID_to_prev_nodes_list.clear();
427 for (
auto partitions_it = m_curr_partitions.begin();
428 partitions_it != m_curr_partitions.end();
429 ++partitions_it, ++partitionID)
433 if (m_lc_params.LC_check_curr_partition_only)
435 bool curr_node_in_curr_partition =
437 partitions_it->begin(), partitions_it->end(),
438 this->m_graph->nodeCount() - 1)) != partitions_it->end());
439 if (!curr_node_in_curr_partition)
446 finder = m_partitionID_to_prev_nodes_list.find(partitionID);
447 if (finder == m_partitionID_to_prev_nodes_list.end())
450 m_partitionID_to_prev_nodes_list.insert(
451 make_pair(partitionID, *partitions_it));
455 if (*partitions_it == finder->second)
465 finder->second = *partitions_it;
470 int curr_node_index = 1;
471 size_t prev_nodeID = *(partitions_it->begin());
472 for (
auto it = partitions_it->begin() + 1; it != partitions_it->end();
473 ++it, ++curr_node_index)
475 size_t curr_nodeID = *it;
479 if ((curr_nodeID - prev_nodeID) > m_lc_params.LC_min_nodeid_diff)
483 int num_after_nodes = partitions_it->size() - curr_node_index;
484 int num_before_nodes = partitions_it->size() - num_after_nodes;
485 if (num_after_nodes >= m_lc_params.LC_min_remote_nodes &&
486 num_before_nodes >= m_lc_params.LC_min_remote_nodes)
489 "Found potential loop closures:"
491 <<
"\tPartitionID: " << partitionID << endl
497 <<
"\t" << prev_nodeID <<
" ==> " << curr_nodeID << endl
498 <<
"\tNumber of LC nodes: " << num_after_nodes);
499 partitions_for_LC->push_back(*partitions_it);
506 prev_nodeID = curr_nodeID;
509 "Successfully checked partition: " << partitionID);
512 this->m_time_logger.leave(
"LoopClosureEvaluation");
516 template <
class GRAPH_T>
521 using namespace mrpt;
525 this->m_time_logger.enter(
"LoopClosureEvaluation");
527 if (partitions.size() == 0)
return;
530 "Evaluating partitions for loop closures...\n%s\n",
531 this->header_sep.c_str());
534 for (
auto partition : partitions)
537 std::vector<uint32_t> groupA, groupB;
538 this->splitPartitionToGroups(partition, &groupA, &groupB, 5);
542 this->generateHypotsPool(groupA, groupB, &hypots_pool);
545 CMatrixDouble consist_matrix(hypots_pool.size(), hypots_pool.size());
546 this->generatePWConsistenciesMatrix(
547 groupA, groupB, hypots_pool, &consist_matrix);
551 this->evalPWConsistenciesMatrix(
552 consist_matrix, hypots_pool, &valid_hypots);
555 if (valid_hypots.size())
558 for (
auto it = valid_hypots.begin(); it != valid_hypots.end(); ++it)
560 this->registerHypothesis(**it);
565 for (
auto it = hypots_pool.begin(); it != hypots_pool.end(); ++it)
572 this->m_time_logger.leave(
"LoopClosureEvaluation");
577 template <
class GRAPH_T>
587 valid_hypots->clear();
592 bool valid_lambda_ratio =
593 this->computeDominantEigenVector(consist_matrix, &u,
false);
594 if (!valid_lambda_ratio)
return;
602 double dot_product = 0;
603 for (
int i = 0; i != w.
size(); ++i)
610 double potential_dot_product =
614 "current: %f | potential_dot_product: %f", dot_product,
615 potential_dot_product);
616 if (potential_dot_product > dot_product)
619 dot_product = potential_dot_product;
629 cout <<
"Outcome of discretization: " << w.
transpose() << endl;
635 for (
int wi = 0; wi != w.
size(); ++wi)
642 valid_hypots->push_back(this->findHypotByID(hypots_pool, wi));
651 template <
class GRAPH_T>
653 std::vector<uint32_t>& partition, std::vector<uint32_t>* groupA,
654 std::vector<uint32_t>* groupB,
int max_nodes_in_group)
658 using namespace mrpt;
666 max_nodes_in_group == -1 || max_nodes_in_group > 0,
668 "Value %d not permitted for max_nodes_in_group"
669 "Either use a positive integer, "
670 "or -1 for non-restrictive partition size",
671 max_nodes_in_group));
676 size_t index_to_split = 1;
677 for (
auto it = partition.begin() + 1; it != partition.end();
678 ++it, ++index_to_split)
682 if ((curr_nodeID - prev_nodeID) > m_lc_params.LC_min_nodeid_diff)
687 prev_nodeID = curr_nodeID;
689 ASSERTDEB_(partition.size() > index_to_split);
692 *groupA = std::vector<uint32_t>(
693 partition.begin(), partition.begin() + index_to_split);
694 *groupB = std::vector<uint32_t>(
695 partition.begin() + index_to_split, partition.end());
697 if (max_nodes_in_group != -1)
700 if (groupA->size() >
static_cast<size_t>(max_nodes_in_group))
702 *groupA = std::vector<uint32_t>(
703 groupA->begin(), groupA->begin() + max_nodes_in_group);
706 if (groupB->size() >
static_cast<size_t>(max_nodes_in_group))
708 *groupB = std::vector<uint32_t>(
709 groupB->end() - max_nodes_in_group, groupB->end());
717 template <
class GRAPH_T>
719 const std::vector<uint32_t>& groupA,
const std::vector<uint32_t>& groupB,
723 using namespace mrpt;
728 "generateHypotsPool: Given hypotsp_t pointer is invalid.");
729 generated_hypots->clear();
734 <<
" - size: " << groupA.size() << endl);
737 <<
" - size: " << groupB.size() << endl);
751 size_t nodes_count = groupA.size();
755 nodes_count == p.size(),
757 "Size mismatch between nodeIDs in group [%lu]"
758 " and corresponding properties map [%lu]",
759 nodes_count, p.size()));
765 int hypot_counter = 0;
766 int invalid_hypots = 0;
769 for (
unsigned int b_it : groupB)
771 for (
unsigned int a_it : groupA)
780 hypot->id = hypot_counter++;
795 fillNodePropsFromGroupParams(
799 fillNodePropsFromGroupParams(
821 bool found_edge = this->getICPEdge(
822 b_it, a_it, &edge, &icp_info, icp_ad_params);
824 hypot->setEdge(edge);
831 double goodness_thresh =
832 m_laser_params.goodness_threshold_win.getMedian() *
833 m_lc_icp_constraint_factor;
834 bool accept_goodness = icp_info.
goodness > goodness_thresh;
836 "generateHypotsPool:\nCurr. Goodness: "
837 << icp_info.
goodness <<
"|\t Threshold: " << goodness_thresh
838 <<
" => " << (accept_goodness ?
"ACCEPT" :
"REJECT")
841 if (!found_edge || !accept_goodness)
843 hypot->is_valid =
false;
846 generated_hypots->push_back(hypot);
851 delete icp_ad_params;
855 "Generated pool of hypotheses...\tsize = "
856 << generated_hypots->size()
857 <<
"\tinvalid hypotheses: " << invalid_hypots);
863 template <
class GRAPH_T>
869 using namespace mrpt;
874 this->m_time_logger.enter(
"DominantEigenvectorComputation");
876 double lambda1, lambda2;
877 bool is_valid_lambda_ratio =
false;
879 if (use_power_method)
882 "\nPower method for computing the first two "
883 "eigenvectors/eigenvalues hasn't been implemented yet\n");
888 std::vector<double> eigvals;
889 consist_matrix.
eig(eigvecs, eigvals);
894 eigvecs.
size() == eigvals.size() &&
895 consist_matrix.
cols() == eigvals.size(),
897 "Size of eigvecs \"%lu\","
899 "consist_matrix \"%lu\" don't match",
900 static_cast<unsigned long>(eigvecs.
size()),
901 static_cast<unsigned long>(eigvals.size()),
902 static_cast<unsigned long>(consist_matrix.
size())));
905 for (
int i = 0; i != eigvec->
size(); ++i)
906 (*eigvec)[i] = std::abs(eigvecs(i, eigvecs.
cols() - 1));
908 lambda1 = eigvals[eigvals.size() - 1];
909 lambda2 = eigvals[eigvals.size() - 2];
917 "Bad lambda2 value: "
918 << lambda2 <<
" => Skipping current evaluation." << endl);
921 double curr_lambda_ratio = lambda1 / lambda2;
923 "lambda1 = " << lambda1 <<
" | lambda2 = " << lambda2
924 <<
"| ratio = " << curr_lambda_ratio << endl);
926 is_valid_lambda_ratio =
927 (curr_lambda_ratio > m_lc_params.LC_eigenvalues_ratio_thresh);
929 this->m_time_logger.leave(
"DominantEigenvectorComputation");
930 return is_valid_lambda_ratio;
935 template <
class GRAPH_T>
937 const std::vector<uint32_t>& groupA,
const std::vector<uint32_t>& groupB,
943 using namespace mrpt;
950 consist_matrix,
"Invalid pointer to the Consistency matrix is given");
952 static_cast<unsigned long>(consist_matrix->
rows()) ==
953 hypots_pool.size() &&
954 static_cast<unsigned long>(consist_matrix->
rows()) ==
956 "Consistency matrix dimensions aren't equal to the hypotheses pool "
960 ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
962 <<
"In generatePWConsistencyMatrix:\n"
965 <<
"\tHypots pool Size: " << hypots_pool.size());
968 for (
auto b1_it = groupB.begin(); b1_it != groupB.end(); ++b1_it)
973 for (
auto b2_it = b1_it + 1; b2_it != groupB.end(); ++b2_it)
978 for (
auto a1_it = groupA.begin(); a1_it != groupA.end(); ++a1_it)
982 this->findHypotByEnds(hypots_pool,
b2,
a1);
987 for (
auto a2_it = a1_it + 1; a2_it != groupA.end(); ++a2_it)
991 this->findHypotByEnds(hypots_pool,
b1,
a2);
998 if (hypot_b2_a1->is_valid && hypot_b1_a2->is_valid)
1004 extracted_hypots.push_back(hypot_b2_a1);
1005 extracted_hypots.push_back(hypot_b1_a2);
1007 paths_t* curr_opt_paths =
nullptr;
1008 if (groupA_opt_paths || groupB_opt_paths)
1010 curr_opt_paths =
new paths_t();
1017 if (groupA_opt_paths)
1019 const path_t* p = this->findPathByEnds(
1020 *groupA_opt_paths,
a1,
a2,
true);
1021 curr_opt_paths->push_back(*p);
1025 curr_opt_paths->push_back(
path_t());
1028 if (groupB_opt_paths)
1030 const path_t* p = this->findPathByEnds(
1031 *groupB_opt_paths,
b1,
b2,
true);
1032 curr_opt_paths->push_back(*p);
1036 curr_opt_paths->push_back(
path_t());
1040 consistency = this->generatePWConsistencyElement(
1041 a1,
a2,
b1,
b2, extracted_hypots, curr_opt_paths);
1043 delete curr_opt_paths;
1058 int id1 = hypot_b2_a1->id;
1059 int id2 = hypot_b1_a2->id;
1061 (*consist_matrix)(id1, id2) = consistency;
1062 (*consist_matrix)(id2, id1) = consistency;
1079 template <
class GRAPH_T>
1086 using namespace std;
1087 using namespace mrpt;
1115 const path_t* path_a1_a2;
1116 if (!opt_paths || opt_paths->begin()->isEmpty())
1119 "Running dijkstra [a1] " <<
a1 <<
" => [a2] " <<
a2);
1120 execDijkstraProjection(
a1,
a2);
1121 path_a1_a2 = this->queryOptimalPath(
a2);
1126 path_a1_a2 = &(*opt_paths->begin());
1132 const path_t* path_b1_b2;
1133 if (!opt_paths || opt_paths->rend()->isEmpty())
1136 "Running djkstra [b1] " <<
b1 <<
" => [b2] " <<
b2);
1137 execDijkstraProjection(
b1,
b2);
1138 path_b1_b2 = this->queryOptimalPath(
b2);
1142 path_b1_b2 = &(*opt_paths->rbegin());
1150 hypot_t* hypot_b1_a2 = this->findHypotByEnds(hypots,
b1,
a2);
1152 hypot_t* hypot_b2_a1 = this->findHypotByEnds(hypots,
b2,
a1);
1157 res_transform += hypot_b1_a2->getInverseEdge();
1159 res_transform += hypot_b2_a1->getEdge();
1162 "\n-----------Resulting Transformation----------- Hypots: #"
1163 << hypot_b1_a2->id <<
", #" << hypot_b2_a1->id << endl
1164 <<
"a1 --> a2 => b1 --> b2 => a1: " <<
a1 <<
" --> " <<
a2 <<
" => "
1165 <<
b1 <<
" --> " <<
b2 <<
" => " <<
a1 << endl
1166 << res_transform << endl
1168 <<
"DIJKSTRA: " <<
a1 <<
" --> " <<
a2 <<
": "
1170 <<
"DIJKSTRA: " <<
b1 <<
" --> " <<
b2 <<
": "
1172 <<
"hypot_b1_a2(inv):\n"
1173 << hypot_b1_a2->getInverseEdge() << endl
1175 << hypot_b2_a1->getEdge() << endl);
1178 typename pose_t::vector_t T;
1179 res_transform.getMeanVal().asVector(T);
1183 res_transform.getCovariance(cov_mat);
1190 double consistency_elem = exp(exponent);
1197 return consistency_elem;
1201 template <
class GRAPH_T>
1207 using namespace mrpt;
1210 const path_t* res =
nullptr;
1212 for (
auto cit = vec_paths.begin(); cit != vec_paths.end(); ++cit)
1214 if (cit->getSource() == src && cit->getDestination() == dst)
1220 if (throw_exc && !res)
1223 "Path for %lu => %lu is not found. Exiting...\n",
1224 static_cast<unsigned long>(src),
static_cast<unsigned long>(dst)));
1230 template <
class GRAPH_T>
1237 using namespace std;
1239 for (
auto v_cit = vec_hypots.begin(); v_cit != vec_hypots.end(); v_cit++)
1241 if ((*v_cit)->hasEnds(from, to))
1260 template <
class GRAPH_T>
1263 const hypotsp_t& vec_hypots,
size_t id,
bool throw_exc)
1267 for (
auto v_cit = vec_hypots.begin(); v_cit != vec_hypots.end(); v_cit++)
1269 if ((*v_cit)->id ==
id)
1286 template <
class GRAPH_T>
1291 using namespace std;
1292 using namespace mrpt;
1300 this->m_time_logger.enter(
"Dijkstra Projection");
1301 const std::string dijkstra_end =
1302 "----------- Done with Dijkstra Projection... ----------";
1308 (ending_node >= 0 && ending_node < this->m_graph->nodeCount()));
1310 starting_node != ending_node,
"Starting and Ending nodes coincede");
1313 stringstream ss_debug(
"");
1314 ss_debug <<
"Executing Dijkstra Projection: " << starting_node <<
" => ";
1317 ss_debug <<
"..." << endl;
1321 ss_debug << ending_node << endl;
1324 if (this->m_graph->nodeCount() < m_dijkstra_node_count_thresh)
1330 std::vector<bool> visited_nodes(this->m_graph->nodeCount(),
false);
1331 m_node_optimal_paths.clear();
1334 std::map<TNodeID, std::set<TNodeID>> neighbors_of;
1335 this->m_graph->getAdjacencyMatrix(neighbors_of);
1340 std::set<path_t*> pool_of_paths;
1342 std::set<TNodeID> starting_node_neighbors(neighbors_of.at(starting_node));
1343 for (
unsigned long starting_node_neighbor : starting_node_neighbors)
1345 auto* path_between_neighbors =
new path_t();
1346 this->getMinUncertaintyPath(
1347 starting_node, starting_node_neighbor, path_between_neighbors);
1349 pool_of_paths.insert(path_between_neighbors);
1352 visited_nodes.at(starting_node) =
true;
1369 for (
auto it = visited_nodes.begin(); it != visited_nodes.end(); ++it)
1382 if (visited_nodes.at(ending_node))
1385 this->m_time_logger.leave(
"Dijkstra Projection");
1390 path_t* optimal_path = this->popMinUncertaintyPath(&pool_of_paths);
1415 if (!visited_nodes.at(dest))
1417 m_node_optimal_paths[dest] = optimal_path;
1418 visited_nodes.at(dest) =
true;
1424 &pool_of_paths, *optimal_path, neighbors_of.at(dest));
1429 this->m_time_logger.leave(
"Dijkstra Projection");
1433 template <
class GRAPH_T>
1435 std::set<path_t*>* pool_of_paths,
const path_t& current_path,
1436 const std::set<mrpt::graphs::TNodeID>& neighbors)
const
1439 using namespace std;
1450 for (
unsigned long neighbor : neighbors)
1452 if (neighbor == second_to_last_node)
continue;
1455 path_t path_between_nodes;
1456 this->getMinUncertaintyPath(
1457 node_to_append_from, neighbor, &path_between_nodes);
1460 auto* path_to_append =
new path_t();
1461 *path_to_append = current_path;
1462 *path_to_append += path_between_nodes;
1464 pool_of_paths->insert(path_to_append);
1470 template <
class GRAPH_T>
1475 using namespace std;
1478 typename std::map<mrpt::graphs::TNodeID, path_t*>::const_iterator search;
1479 search = m_node_optimal_paths.find(node);
1480 if (search != m_node_optimal_paths.end())
1482 path = search->second;
1490 template <
class GRAPH_T>
1493 path_t* path_between_nodes)
const
1497 using namespace std;
1500 this->m_graph->edgeExists(from, to) ||
1501 this->m_graph->edgeExists(to, from),
1503 "\nEdge between the provided nodeIDs"
1504 "(%lu <-> %lu) does not exist\n",
1511 path_between_nodes->
clear();
1515 double curr_determinant = 0;
1517 std::pair<edges_citerator, edges_citerator> fwd_edges_pair =
1518 this->m_graph->getEdges(from, to);
1527 for (
auto edges_it = fwd_edges_pair.first;
1528 edges_it != fwd_edges_pair.second; ++edges_it)
1533 curr_edge.copyFrom(edges_it->second);
1537 curr_edge.getInformationMatrix(inf_mat);
1542 curr_edge.cov_inv = inf_mat;
1553 *path_between_nodes = curr_path;
1557 std::pair<edges_citerator, edges_citerator> bwd_edges_pair =
1558 this->m_graph->getEdges(to, from);
1567 for (
auto edges_it = bwd_edges_pair.first;
1568 edges_it != bwd_edges_pair.second; ++edges_it)
1573 (edges_it->second).inverse(curr_edge);
1577 curr_edge.getInformationMatrix(inf_mat);
1582 curr_edge.cov_inv = inf_mat;
1593 *path_between_nodes = curr_path;
1600 template <
class GRAPH_T>
1602 typename std::set<path_t*>* pool_of_paths)
const
1605 using namespace std;
1608 path_t* optimal_path =
nullptr;
1609 double curr_determinant = 0;
1610 for (
auto it = pool_of_paths->begin(); it != pool_of_paths->end(); ++it)
1615 if (curr_determinant < (*it)->getDeterminant())
1623 pool_of_paths->erase(optimal_path);
1625 return optimal_path;
1629 template <
class GRAPH_T>
1636 using namespace std;
1641 this->m_graph->nodes.at(to) - this->m_graph->nodes.at(from);
1642 typename pose_t::vector_t mean_diff;
1643 (rel_edge.getMeanVal() - initial_estim).asVector(mean_diff);
1647 rel_edge.getCovariance(cov_mat);
1650 double mahal_distance =
1652 bool mahal_distance_null = std::isnan(mahal_distance);
1653 if (!mahal_distance_null)
1655 m_laser_params.mahal_distance_ICP_odom_win.addNewMeasurement(
1662 m_laser_params.mahal_distance_ICP_odom_win.getMedian() * 4;
1664 (threshold >= mahal_distance && !mahal_distance_null) ?
true :
false;
1676 template <
class GRAPH_T>
1681 this->registerNewEdge(hypot.from, hypot.to, hypot.getEdge());
1684 template <
class GRAPH_T>
1691 using namespace std;
1692 parent_t::registerNewEdge(from, to, rel_edge);
1695 m_edge_types_to_nums[
"ICP2D"]++;
1698 if (
absDiff(to, from) > m_lc_params.LC_min_nodeid_diff)
1700 m_edge_types_to_nums[
"LC"]++;
1701 this->m_just_inserted_lc =
true;
1706 this->m_just_inserted_lc =
false;
1710 this->m_graph->insertEdge(from, to, rel_edge);
1715 template <
class GRAPH_T>
1720 parent_t::setWindowManagerPtr(win_manager);
1723 if (this->m_win_manager)
1725 if (this->m_win_observer)
1727 this->m_win_observer->registerKeystroke(
1728 m_laser_params.keystroke_laser_scans,
1729 "Toggle LaserScans Visualization");
1730 this->m_win_observer->registerKeystroke(
1731 m_lc_params.keystroke_map_partitions,
1732 "Toggle Map Partitions Visualization");
1736 "Fetched the window manager, window observer successfully.");
1739 template <
class GRAPH_T>
1741 const std::map<std::string, bool>& events_occurred)
1744 parent_t::notifyOfWindowEvents(events_occurred);
1747 if (events_occurred.at(m_laser_params.keystroke_laser_scans))
1749 this->toggleLaserScansVisualization();
1752 if (events_occurred.at(m_lc_params.keystroke_map_partitions))
1754 this->toggleMapPartitionsVisualization();
1760 template <
class GRAPH_T>
1763 using namespace mrpt;
1769 this->m_win_manager->assignTextMessageParameters(
1770 &m_lc_params.offset_y_map_partitions,
1771 &m_lc_params.text_index_map_partitions);
1775 map_partitions_obj->setName(
"map_partitions");
1778 scene->insert(map_partitions_obj);
1779 this->m_win->unlockAccess3DScene();
1780 this->m_win->forceRepaint();
1783 template <
class GRAPH_T>
1786 using namespace mrpt;
1793 std::stringstream title;
1794 title <<
"# Partitions: " << m_curr_partitions.size();
1795 this->m_win_manager->addTextMessage(
1796 5, -m_lc_params.offset_y_map_partitions, title.str(),
1798 m_lc_params.text_index_map_partitions);
1809 map_partitions_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1812 int partitionID = 0;
1813 bool partition_contains_last_node =
false;
1814 bool found_last_node =
1817 "Searching for the partition of the last nodeID: "
1818 << (this->m_graph->nodeCount() - 1));
1820 for (
auto p_it = m_curr_partitions.begin(); p_it != m_curr_partitions.end();
1821 ++p_it, ++partitionID)
1824 std::vector<uint32_t> nodes_list = *p_it;
1828 nodes_list.begin(), nodes_list.end(),
1829 this->m_graph->nodeCount() - 1) != nodes_list.end())
1831 partition_contains_last_node =
true;
1833 found_last_node =
true;
1837 partition_contains_last_node =
false;
1841 std::string partition_obj_name =
1843 std::string balloon_obj_name =
mrpt::format(
"#%d", partitionID);
1846 map_partitions_obj->getByName(partition_obj_name);
1853 curr_partition_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1854 if (m_lc_params.LC_check_curr_partition_only)
1856 curr_partition_obj->setVisibility(partition_contains_last_node);
1862 "\tCreating a new CSetOfObjects partition object for partition "
1865 curr_partition_obj = std::make_shared<CSetOfObjects>();
1866 curr_partition_obj->setName(partition_obj_name);
1867 if (m_lc_params.LC_check_curr_partition_only)
1870 curr_partition_obj->setVisibility(partition_contains_last_node);
1875 CSphere::Ptr balloon_obj = std::make_shared<CSphere>();
1876 balloon_obj->setName(balloon_obj_name);
1877 balloon_obj->setRadius(m_lc_params.balloon_radius);
1878 balloon_obj->setColor_u8(m_lc_params.balloon_std_color);
1879 balloon_obj->enableShowName();
1881 curr_partition_obj->insert(balloon_obj);
1887 std::make_shared<CSetOfLines>();
1888 connecting_lines_obj->setName(
"connecting_lines");
1889 connecting_lines_obj->setColor_u8(
1890 m_lc_params.connecting_lines_color);
1891 connecting_lines_obj->setLineWidth(0.1f);
1893 curr_partition_obj->insert(connecting_lines_obj);
1898 map_partitions_obj->insert(curr_partition_obj);
1905 std::pair<double, double> centroid_coords;
1906 this->computeCentroidOfNodesVector(nodes_list, ¢roid_coords);
1909 centroid_coords.first, centroid_coords.second,
1910 m_lc_params.balloon_elevation);
1919 curr_partition_obj->getByName(balloon_obj_name);
1920 balloon_obj = std::dynamic_pointer_cast<CSphere>(_obj);
1921 balloon_obj->setLocation(balloon_location);
1922 if (partition_contains_last_node)
1923 balloon_obj->setColor_u8(m_lc_params.balloon_curr_color);
1925 balloon_obj->setColor_u8(m_lc_params.balloon_std_color);
1938 curr_partition_obj->getByName(
"connecting_lines");
1939 connecting_lines_obj = std::dynamic_pointer_cast<CSetOfLines>(_obj);
1941 connecting_lines_obj->clear();
1943 for (
auto it = nodes_list.begin(); it != nodes_list.end(); ++it)
1945 CPose3D curr_pose(this->m_graph->nodes.at(*it));
1949 curr_node_location, balloon_location);
1950 connecting_lines_obj->appendLine(connecting_line);
1956 if (!found_last_node)
1959 THROW_EXCEPTION(
"Last inserted nodeID was not found in any partition.");
1967 const size_t prev_size = m_last_partitions.size();
1968 const size_t curr_size = m_curr_partitions.size();
1969 if (curr_size < prev_size)
1972 for (
size_t pID = curr_size; pID != prev_size; ++pID)
1975 std::string partition_obj_name =
1976 mrpt::format(
"partition_%lu",
static_cast<unsigned long>(pID));
1979 map_partitions_obj->getByName(partition_obj_name);
1983 "Partition: %s was not found", partition_obj_name.c_str());
1985 map_partitions_obj->removeObject(obj);
1990 this->m_win->unlockAccess3DScene();
1991 this->m_win->forceRepaint();
1994 template <
class GRAPH_T>
1998 ASSERTDEBMSG_(this->m_win,
"No CDisplayWindow3D* was provided");
1999 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* was provided");
2005 if (m_lc_params.visualize_map_partitions)
2008 scene->getByName(
"map_partitions");
2009 obj->setVisibility(!obj->isVisible());
2013 this->dumpVisibilityErrorMsg(
"visualize_map_partitions");
2016 this->m_win->unlockAccess3DScene();
2017 this->m_win->forceRepaint();
2022 template <
class GRAPH_T>
2024 const std::vector<uint32_t>& nodes_list,
2025 std::pair<double, double>* centroid_coords)
const
2031 double centroid_x = 0;
2032 double centroid_y = 0;
2033 for (
unsigned int node_it : nodes_list)
2035 pose_t curr_node_pos = this->m_graph->nodes.at(node_it);
2036 centroid_x += curr_node_pos.x();
2037 centroid_y += curr_node_pos.y();
2041 centroid_coords->first =
2042 centroid_x /
static_cast<double>(nodes_list.size());
2043 centroid_coords->second =
2044 centroid_y /
static_cast<double>(nodes_list.size());
2049 template <
class GRAPH_T>
2055 if (m_laser_params.visualize_laser_scans)
2058 this->m_win->get3DSceneAndLock();
2062 laser_scan_viz->enablePoints(
true);
2063 laser_scan_viz->enableLine(
true);
2064 laser_scan_viz->enableSurface(
true);
2065 laser_scan_viz->setSurfaceColor(
2066 m_laser_params.laser_scans_color.R,
2067 m_laser_params.laser_scans_color.G,
2068 m_laser_params.laser_scans_color.B,
2069 m_laser_params.laser_scans_color.A);
2071 laser_scan_viz->setName(
"laser_scan_viz");
2073 scene->insert(laser_scan_viz);
2074 this->m_win->unlockAccess3DScene();
2075 this->m_win->forceRepaint();
2081 template <
class GRAPH_T>
2087 if (m_laser_params.visualize_laser_scans && m_last_laser_scan2D)
2090 this->m_win->get3DSceneAndLock();
2093 scene->getByName(
"laser_scan_viz");
2095 std::dynamic_pointer_cast<mrpt::opengl::CPlanarLaserScan>(obj);
2096 laser_scan_viz->setScan(*m_last_laser_scan2D);
2100 this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
2101 if (search != this->m_graph->nodes.end())
2103 laser_scan_viz->setPose(search->second);
2107 laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(), -0.15,
2113 this->m_win->unlockAccess3DScene();
2114 this->m_win->forceRepaint();
2120 template <
class GRAPH_T>
2124 ASSERTDEBMSG_(this->m_win,
"No CDisplayWindow3D* was provided");
2125 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* was provided");
2131 if (m_laser_params.visualize_laser_scans)
2134 scene->getByName(
"laser_scan_viz");
2135 obj->setVisibility(!obj->isVisible());
2139 this->dumpVisibilityErrorMsg(
"visualize_laser_scans");
2142 this->m_win->unlockAccess3DScene();
2143 this->m_win->forceRepaint();
2148 template <
class GRAPH_T>
2150 std::map<std::string, int>* edge_types_to_num)
const
2153 *edge_types_to_num = m_edge_types_to_nums;
2157 template <
class GRAPH_T>
2161 parent_t::initializeVisuals();
2163 this->m_time_logger.enter(
"Visuals");
2166 m_laser_params.has_read_config,
2167 "Configuration parameters aren't loaded yet");
2168 if (m_laser_params.visualize_laser_scans)
2170 this->initLaserScansVisualization();
2172 if (m_lc_params.visualize_map_partitions)
2174 this->initMapPartitionsVisualization();
2177 if (m_visualize_curr_node_covariance)
2179 this->initCurrCovarianceVisualization();
2182 this->m_time_logger.leave(
"Visuals");
2185 template <
class GRAPH_T>
2189 parent_t::updateVisuals();
2191 this->m_time_logger.enter(
"Visuals");
2193 if (m_laser_params.visualize_laser_scans)
2195 this->updateLaserScansVisualization();
2197 if (m_lc_params.visualize_map_partitions)
2199 this->updateMapPartitionsVisualization();
2201 if (m_visualize_curr_node_covariance)
2203 this->updateCurrCovarianceVisualization();
2206 this->m_time_logger.leave(
"Visuals");
2210 template <
class GRAPH_T>
2214 using namespace std;
2218 this->m_win_manager->assignTextMessageParameters(
2219 &m_offset_y_curr_node_covariance, &m_text_index_curr_node_covariance);
2221 std::string title(
"Position uncertainty");
2222 this->m_win_manager->addTextMessage(
2223 5, -m_offset_y_curr_node_covariance, title,
2225 m_text_index_curr_node_covariance);
2229 cov_ellipsis_obj->setName(
"cov_ellipsis_obj");
2230 cov_ellipsis_obj->setColor_u8(m_curr_node_covariance_color);
2231 cov_ellipsis_obj->setLocation(0, 0, 0);
2235 scene->insert(cov_ellipsis_obj);
2236 this->m_win->unlockAccess3DScene();
2237 this->m_win->forceRepaint();
2242 template <
class GRAPH_T>
2246 using namespace std;
2253 path_t* path = queryOptimalPath(curr_node);
2258 pose_t curr_position = this->m_graph->nodes.at(curr_node);
2261 "In updateCurrCovarianceVisualization\n"
2262 "Covariance matrix:\n"
2271 std::dynamic_pointer_cast<CEllipsoid3D>(obj);
2274 cov_ellipsis_obj->setLocation(curr_position.x(), curr_position.y(), 0);
2277 cov_ellipsis_obj->setCovMatrix(mat);
2279 this->m_win->unlockAccess3DScene();
2280 this->m_win->forceRepaint();
2285 template <
class GRAPH_T>
2287 std::string viz_flag,
int sleep_time)
2293 "Cannot toggle visibility of specified object.\n "
2294 "Make sure that the corresponding visualization flag ( %s "
2295 ") is set to true in the .ini file.\n",
2297 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
2302 template <
class GRAPH_T>
2306 parent_t::loadParams(source_fname);
2308 m_partitioner.options.loadFromConfigFileName(
2309 source_fname,
"EdgeRegistrationDeciderParameters");
2310 m_laser_params.loadFromConfigFileName(
2311 source_fname,
"EdgeRegistrationDeciderParameters");
2312 m_lc_params.loadFromConfigFileName(
2313 source_fname,
"EdgeRegistrationDeciderParameters");
2317 m_consec_icp_constraint_factor = source.
read_double(
2318 "EdgeRegistrationDeciderParameters",
"consec_icp_constraint_factor",
2321 "EdgeRegistrationDeciderParameters",
"lc_icp_constraint_factor", 0.70,
2325 int min_verbosity_level = source.
read_int(
2326 "EdgeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
2331 template <
class GRAPH_T>
2335 using namespace std;
2337 cout <<
"------------------[Pair-wise Consistency of ICP Edges - "
2338 "Registration Procedure Summary]------------------"
2341 parent_t::printParams();
2342 m_partitioner.options.dumpToConsole();
2343 m_laser_params.dumpToConsole();
2344 m_lc_params.dumpToConsole();
2346 cout <<
"Scan-matching ICP Constraint factor: "
2347 << m_consec_icp_constraint_factor << endl;
2348 cout <<
"Loop-closure ICP Constraint factor: "
2349 << m_lc_icp_constraint_factor << endl;
2355 template <
class GRAPH_T>
2357 std::string* report_str)
const
2362 std::stringstream class_props_ss;
2363 class_props_ss <<
"Pair-wise Consistency of ICP Edges - Registration "
2364 "Procedure Summary: "
2366 class_props_ss << this->header_sep << std::endl;
2369 const std::string time_res = this->m_time_logger.getStatsAsText();
2370 const std::string output_res = this->getLogAsString();
2373 report_str->clear();
2374 parent_t::getDescriptiveReport(report_str);
2376 *report_str += class_props_ss.str();
2377 *report_str += this->report_sep;
2379 *report_str += time_res;
2380 *report_str += this->report_sep;
2382 *report_str += output_res;
2383 *report_str += this->report_sep;
2388 template <
class GRAPH_T>
2392 partitions_out = this->getCurrentPartitions();
2395 template <
class GRAPH_T>
2396 const std::vector<std::vector<uint32_t>>&
2399 return m_curr_partitions;
2402 template <
class GRAPH_T>
2404 bool full_update,
bool is_first_time_node_reg)
2408 using namespace std;
2409 this->m_time_logger.enter(
"updateMapPartitions");
2416 "updateMapPartitions: Full partitioning of map was issued");
2419 m_partitioner.clear();
2420 nodes_to_scans = this->m_nodes_to_laser_scans2D;
2425 if (is_first_time_node_reg)
2427 nodes_to_scans.insert(make_pair(
2428 this->m_graph->root,
2429 this->m_nodes_to_laser_scans2D.at(this->m_graph->root)));
2433 nodes_to_scans.insert(make_pair(
2434 this->m_graph->nodeCount() - 1,
2435 this->m_nodes_to_laser_scans2D.at(this->m_graph->nodeCount() - 1)));
2441 for (
auto it = nodes_to_scans.begin(); it != nodes_to_scans.end(); ++it)
2446 "nodeID \"" << it->first <<
"\" has invalid laserScan");
2452 const auto& search = this->m_graph->nodes.find(it->first);
2453 if (search == this->m_graph->nodes.end())
2460 const auto curr_constraint =
constraint_t(search->second);
2468 m_partitioner.addMapFrame(sf, *pose3d);
2472 size_t curr_size = m_curr_partitions.size();
2473 m_last_partitions.resize(curr_size);
2474 for (
size_t i = 0; i < curr_size; i++)
2476 m_last_partitions[i] = m_curr_partitions[i];
2479 m_partitioner.updatePartitions(m_curr_partitions);
2482 this->m_time_logger.leave(
"updateMapPartitions");
2489 template <
class GRAPH_T>
2492 mahal_distance_ICP_odom_win.resizeWindow(
2494 goodness_threshold_win.resizeWindow(200);
2497 template <
class GRAPH_T>
2500 template <
class GRAPH_T>
2502 std::ostream&
out)
const
2506 out <<
"Use scan-matching constraints = "
2507 << (use_scan_matching ?
"TRUE" :
"FALSE") << std::endl;
2508 out <<
"Num. of previous nodes to check ICP against = "
2509 << prev_nodes_for_ICP << std::endl;
2510 out <<
"Visualize laser scans = "
2511 << (visualize_laser_scans ?
"TRUE" :
"FALSE") << std::endl;
2515 template <
class GRAPH_T>
2522 source.
read_bool(section,
"use_scan_matching",
true,
false);
2523 prev_nodes_for_ICP =
2525 section,
"prev_nodes_for_ICP", 10,
false);
2526 visualize_laser_scans = source.
read_bool(
2527 "VisualizationParameters",
"visualize_laser_scans",
true,
false);
2529 has_read_config =
true;
2535 template <
class GRAPH_T>
2537 : keystroke_map_partitions(
"b"),
2539 balloon_std_color(153, 0, 153),
2540 balloon_curr_color(62, 0, 80),
2541 connecting_lines_color(balloon_std_color)
2546 template <
class GRAPH_T>
2549 template <
class GRAPH_T>
2551 std::ostream&
out)
const
2554 using namespace std;
2557 ss <<
"Min. node difference for loop closure = "
2558 << LC_min_nodeid_diff << endl;
2559 ss <<
"Remote NodeIDs to consider the potential loop closure = "
2560 << LC_min_remote_nodes << endl;
2561 ss <<
"Min EigenValues ratio for accepting a hypotheses set = "
2562 << LC_eigenvalues_ratio_thresh << endl;
2563 ss <<
"Check only current node's partition for loop closures = "
2564 << (LC_check_curr_partition_only ?
"TRUE" :
"FALSE") << endl;
2565 ss <<
"New registered nodes required for full partitioning = "
2566 << full_partition_per_nodes << endl;
2567 ss <<
"Visualize map partitions = "
2568 << (visualize_map_partitions ?
"TRUE" :
"FALSE") << endl;
2574 template <
class GRAPH_T>
2579 LC_min_nodeid_diff = source.
read_int(
2580 "GeneralConfiguration",
"LC_min_nodeid_diff", 30,
false);
2581 LC_min_remote_nodes =
2582 source.
read_int(section,
"LC_min_remote_nodes", 3,
false);
2583 LC_eigenvalues_ratio_thresh =
2584 source.
read_double(section,
"LC_eigenvalues_ratio_thresh", 2,
false);
2585 LC_check_curr_partition_only =
2586 source.
read_bool(section,
"LC_check_curr_partition_only",
true,
false);
2587 full_partition_per_nodes =
2588 source.
read_int(section,
"full_partition_per_nodes", 50,
false);
2589 visualize_map_partitions = source.
read_bool(
2590 "VisualizationParameters",
"visualize_map_partitions",
true,
false);
2592 has_read_config =
true;