11 #ifndef CLOOPCLOSERERD_IMPL_H 12 #define CLOOPCLOSERERD_IMPL_H 14 namespace mrpt {
namespace graphslam {
namespace deciders {
18 template<
class GRAPH_T>
20 m_visualize_curr_node_covariance(false),
21 m_curr_node_covariance_color(160, 160, 160, 255),
22 m_partitions_full_update(false),
23 m_is_first_time_node_reg(true),
24 m_dijkstra_node_count_thresh(3)
32 template<
class GRAPH_T>
38 for (
typename std::map<
52 template<
class GRAPH_T>
54 mrpt::obs::CActionCollectionPtr action,
55 mrpt::obs::CSensoryFramePtr observations,
56 mrpt::obs::CObservationPtr observation ) {
70 CObservation2DRangeScanPtr scan =
71 getObservation<CObservation2DRangeScan>(observations, observation);
80 size_t num_registered =
absDiff(
82 bool registered_new_node = num_registered > 0;
84 if (registered_new_node) {
86 registered_new_node =
true;
93 "Invalid number of registered nodes since last call to updateStates| " 94 "Found \"" << num_registered <<
"\" new nodes.");
125 num_registered == 2);
144 template<
class GRAPH_T>
147 std::set<mrpt::utils::TNodeID>* nodes_set) {
152 int fetched_nodeIDs = 0;
153 for (
int nodeID_i = static_cast<int>(curr_nodeID)-1;
157 nodes_set->insert(nodeID_i);
162 template<
class GRAPH_T>
167 using namespace mrpt;
176 std::set<TNodeID> nodes_set;
182 node_it != nodes_set.end(); ++node_it) {
188 <<
"==> " << curr_nodeID);
195 if (!found_edge)
continue;
202 double goodness_thresh =
205 bool accept_goodness = icp_info.
goodness > goodness_thresh;
207 <<
"|\t Threshold: " << goodness_thresh <<
" => " << (accept_goodness?
"ACCEPT" :
"REJECT"));
212 *node_it, curr_nodeID, rel_edge);
215 if (accept_goodness && accept_mahal_distance) {
222 template<
class GRAPH_T>
241 CObservation2DRangeScanPtr from_scan, to_scan;
253 bool from_success = this->
getPropsOfNodeID(from, &from_pose, from_scan, from_params);
259 if (!from_success || !to_success) {
261 "Either node #" << from <<
262 " or node #" << to <<
263 " doesn't contain a valid LaserScan. Ignoring this...");
274 initial_estim = to_pose - from_pose;
278 <<
"| to_pose: " << to_pose
279 <<
"| init_estim: " << initial_estim);
294 template<
class GRAPH_T>
297 const std::map<mrpt::utils::TNodeID, node_props_t>& group_params,
307 search = group_params.find(nodeID);
309 if (search == group_params.end()) {
313 *node_props = search->second;
323 template<
class GRAPH_T>
327 mrpt::obs::CObservation2DRangeScanPtr& scan,
333 bool filled_pose =
false;
334 bool filled_scan =
false;
339 *pose = node_props->
pose;
343 if (node_props->
scan.present()) {
344 scan = node_props->
scan;
351 format(
"Either BOTH or NONE of the filled_pose, filled_scan should be set." 352 "NodeID: [%lu]", static_cast<unsigned long>(nodeID)));
360 this->
m_graph->nodes.find(nodeID);
361 if (search != this->
m_graph->nodes.end()) {
362 *pose = search->second;
373 scan = search->second;
378 return filled_pose && filled_scan;
381 template<
class GRAPH_T>
388 using namespace mrpt;
392 partitions_for_LC->clear();
407 ++partitions_it, ++partitionID)
412 bool curr_node_in_curr_partition =
413 ((
find(partitions_it->begin(),
414 partitions_it->end(),
416 != partitions_it->end());
417 if (!curr_node_in_curr_partition) {
429 if (*partitions_it == finder->second) {
436 finder->second = *partitions_it;
442 int curr_node_index = 1;
443 size_t prev_nodeID = *(partitions_it->begin());
445 it != partitions_it->end(); ++it, ++curr_node_index) {
446 size_t curr_nodeID = *it;
453 int num_after_nodes = partitions_it->size() - curr_node_index;
454 int num_before_nodes = partitions_it->size() - num_after_nodes;
458 "Found potential loop closures:" << endl <<
459 "\tPartitionID: " << partitionID << endl <<
461 <<
"\t" << prev_nodeID <<
" ==> " << curr_nodeID << endl <<
462 "\tNumber of LC nodes: " << num_after_nodes);
463 partitions_for_LC->push_back(*partitions_it);
469 prev_nodeID = curr_nodeID;
471 this->logFmt(LVL_DEBUG,
"Successfully checked partition: %d", partitionID);
478 template<
class GRAPH_T>
482 using namespace mrpt;
489 if (partitions.size() == 0)
return;
491 this->logFmt(LVL_DEBUG,
"Evaluating partitions for loop closures...\n%s\n",
496 p_it != partitions.end();
512 CMatrixDouble consist_matrix(hypots_pool.size(), hypots_pool.size());
514 groupA, groupB, hypots_pool, &consist_matrix);
524 if (valid_hypots.size()) {
527 it = valid_hypots.begin(); it != valid_hypots.end(); ++it) {
534 it = hypots_pool.begin(); it != hypots_pool.end(); ++it) {
545 template<
class GRAPH_T>
556 valid_hypots->clear();
561 bool valid_lambda_ratio =
565 if (!valid_lambda_ratio)
return;
573 double dot_product = 0;
574 for (
int i = 0; i != w.size(); ++i) {
580 double potential_dot_product = ((w.transpose() * u) / w.squaredNorm()).
value();
581 ss <<
mrpt::format(
"current: %f | potential_dot_product: %f",
582 dot_product, potential_dot_product);
583 if (potential_dot_product > dot_product) {
585 dot_product = potential_dot_product;
594 cout <<
"Outcome of discretization: " << w.transpose() << endl;
599 for (
int wi = 0; wi != w.size(); ++wi) {
603 valid_hypots->push_back(this->
findHypotByID(hypots_pool, wi));
612 template<
class GRAPH_T>
617 int max_nodes_in_group) {
620 using namespace mrpt;
625 ASSERTMSG_(groupA,
"Pointer to groupA is not valid");
626 ASSERTMSG_(groupB,
"Pointer to groupB is not valid");
627 ASSERTMSG_(max_nodes_in_group == -1 || max_nodes_in_group > 0,
629 "Value %d not permitted for max_nodes_in_group" 630 "Either use a positive integer, " 631 "or -1 for non-restrictive partition size",
632 max_nodes_in_group));
637 size_t index_to_split = 1;
639 it != partition.end(); ++it, ++index_to_split) {
646 prev_nodeID = curr_nodeID;
648 ASSERT_(partition.size() > index_to_split);
651 *groupA =
vector_uint(partition.begin(), partition.begin() + index_to_split);
652 *groupB =
vector_uint(partition.begin() + index_to_split, partition.end());
654 if (max_nodes_in_group != -1) {
656 if (groupA->size() >
static_cast<size_t>(max_nodes_in_group)) {
658 groupA->begin() + max_nodes_in_group);
661 if (groupB->size() >
static_cast<size_t>(max_nodes_in_group)) {
662 *groupB =
vector_uint(groupB->end() - max_nodes_in_group,
671 template<
class GRAPH_T>
679 using namespace mrpt;
682 "generateHypotsPool: Given hypotsp_t pointer is invalid.");
683 generated_hypots->clear();
687 " - size: " << groupA.size() << endl);
689 " - size: " << groupB.size() << endl);
700 size_t nodes_count = groupA.size();
704 format(
"Size mismatch between nodeIDs in group [%lu]" 705 " and corresponding properties map [%lu]",
715 int hypot_counter = 0;
716 int invalid_hypots = 0;
720 b_it = groupB.begin();
721 b_it != groupB.end();
724 a_it = groupA.begin();
725 a_it != groupA.end();
732 hypot->
id = hypot_counter++;
781 double goodness_thresh =
784 bool accept_goodness = icp_info.
goodness > goodness_thresh;
786 <<
"|\t Threshold: " << goodness_thresh <<
" => " <<
787 (accept_goodness?
"ACCEPT" :
"REJECT") << endl);
789 if (!found_edge || !accept_goodness) {
793 generated_hypots->push_back(hypot);
798 delete icp_ad_params;
802 "Generated pool of hypotheses...\tsize = " 803 << generated_hypots->size()
804 <<
"\tinvalid hypotheses: " << invalid_hypots);
811 template<
class GRAPH_T>
815 bool use_power_method) {
817 using namespace mrpt;
825 double lambda1, lambda2;
826 bool is_valid_lambda_ratio =
false;
828 if (use_power_method) {
830 "\nPower method for computing the first two eigenvectors/eigenvalues hasn't been implemented yet\n");
834 consist_matrix.eigenVectors(eigvecs, eigvals);
839 eigvecs.size() == eigvals.size() &&
840 consist_matrix.size() == eigvals.size(),
842 "Size of eigvecs \"%lu\"," 844 "consist_matrix \"%lu\" don't match",
845 static_cast<unsigned long>(eigvecs.size()),
846 static_cast<unsigned long>(eigvals.size()),
847 static_cast<unsigned long>(consist_matrix.size())));
849 eigvecs.extractCol(eigvecs.getColCount()-1, *eigvec);
850 lambda1 = eigvals(eigvals.getRowCount()-1, eigvals.getColCount()-1);
851 lambda2 = eigvals(eigvals.getRowCount()-2, eigvals.getColCount()-2);
855 for (
int i = 0; i != eigvec->size(); ++i) {
856 (*eigvec)(i) = abs((*eigvec)(i));
863 <<
" => Skipping current evaluation." << endl);
866 double curr_lambda_ratio = lambda1 / lambda2;
868 "lambda1 = " << lambda1 <<
" | lambda2 = " << lambda2
869 <<
"| ratio = " << curr_lambda_ratio << endl);
871 is_valid_lambda_ratio =
875 return is_valid_lambda_ratio;
880 template<
class GRAPH_T>
886 const paths_t* groupA_opt_paths,
887 const paths_t* groupB_opt_paths) {
890 using namespace mrpt;
894 ASSERTMSG_(consist_matrix,
"Invalid pointer to the Consistency matrix is given");
896 static_cast<unsigned long>(consist_matrix->rows()) == hypots_pool.size() &&
897 static_cast<unsigned long>(consist_matrix->rows()) == hypots_pool.size(),
898 "Consistency matrix dimensions aren't equal to the hypotheses pool size");
901 <<
"In generatePWConsistencyMatrix:\n" 904 <<
"\tHypots pool Size: " << hypots_pool.size());
908 b1_it = groupB.begin(); b1_it != groupB.end(); ++b1_it) {
913 b2_it = b1_it+1; b2_it != groupB.end(); ++b2_it) {
918 a1_it = groupA.begin(); a1_it != groupA.end(); ++a1_it) {
926 a2_it = a1_it+1; a2_it != groupA.end(); ++a2_it) {
938 extracted_hypots.push_back(hypot_b2_a1);
939 extracted_hypots.push_back(hypot_b1_a2);
942 if (groupA_opt_paths || groupB_opt_paths) {
943 curr_opt_paths =
new paths_t();
947 if (curr_opt_paths) {
949 if (groupA_opt_paths) {
951 *groupA_opt_paths, a1, a2,
true);
952 curr_opt_paths->push_back(*p);
955 curr_opt_paths->push_back(
path_t());
958 if (groupB_opt_paths) {
960 *groupB_opt_paths, b1, b2,
true);
961 curr_opt_paths->push_back(*p);
964 curr_opt_paths->push_back(
path_t());
974 delete curr_opt_paths;
987 int id1 = hypot_b2_a1->
id;
988 int id2 = hypot_b1_a2->
id;
990 (*consist_matrix)(id1, id2) = consistency;
991 (*consist_matrix)(id2, id1) = consistency;
1009 template<
class GRAPH_T>
1018 using namespace std;
1019 using namespace mrpt;
1039 if (opt_paths) {
ASSERT_(opt_paths->size() == 2); }
1045 const path_t* path_a1_a2;
1046 if (!opt_paths || opt_paths->begin()->isEmpty()) {
1053 path_a1_a2 = &(*opt_paths->begin());
1056 path_a1_a2->assertIsBetweenNodeIDs(a1, a2);
1059 const path_t* path_b1_b2;
1060 if (!opt_paths || opt_paths->rend()->isEmpty()) {
1066 path_b1_b2 = &(*opt_paths->rbegin());
1083 res_transform += hypot_b2_a1->
getEdge();
1086 << hypot_b1_a2->
id <<
", #" << hypot_b2_a1->
id << endl
1087 <<
"a1 --> a2 => b1 --> b2 => a1: " 1088 << a1 <<
" --> " << a2 <<
" => " << b1 <<
" --> " << b2 <<
" => " << a1 << endl
1089 << res_transform << endl << endl
1090 <<
"DIJKSTRA: " << a1 <<
" --> " << a2 <<
": " << path_a1_a2->curr_pose_pdf << endl
1091 <<
"DIJKSTRA: " << b1 <<
" --> " << b2 <<
": " << path_b1_b2->
curr_pose_pdf << endl
1092 <<
"hypot_b1_a2(inv):\n" << hypot_b1_a2->
getInverseEdge() << endl
1093 <<
"hypot_b2_a1:\n" << hypot_b2_a1->
getEdge() << endl);
1097 res_transform.getMeanVal().getAsVector(T);
1101 res_transform.getCovariance(cov_mat);
1106 double exponent = (-T.transpose() * cov_mat * T).
value();
1107 double consistency_elem = exp(exponent);
1114 return consistency_elem;
1118 template<
class GRAPH_T>
1125 using namespace mrpt;
1128 const path_t* res = NULL;
1131 cit = vec_paths.begin();
1132 cit != vec_paths.end();
1135 if (cit->getSource() == src && cit->getDestination() == dst) {
1140 if (throw_exc && !res) {
1142 format(
"Path for %lu => %lu is not found. Exiting...\n",
1143 static_cast<unsigned long>(src),
1144 static_cast<unsigned long>(dst)));
1150 template<
class GRAPH_T>
1158 using namespace std;
1162 v_cit != vec_hypots.end();
1165 if ((*v_cit)->hasEnds(from, to)) {
1181 template<
class GRAPH_T>
1191 v_cit != vec_hypots.end();
1194 if ((*v_cit)->id ==
id) {
1208 template<
class GRAPH_T>
1213 using namespace std;
1214 using namespace mrpt;
1223 const std::string dijkstra_end =
1224 "----------- Done with Dijkstra Projection... ----------";
1229 (ending_node >= 0 && ending_node < this->
m_graph->nodeCount()) );
1230 ASSERTMSG_(starting_node != ending_node,
"Starting and Ending nodes coincede");
1233 stringstream ss_debug(
"");
1234 ss_debug <<
"Executing Dijkstra Projection: " << starting_node <<
" => ";
1236 ss_debug <<
"..." << endl;
1239 ss_debug << ending_node <<endl;
1247 std::vector<bool> visited_nodes(this->
m_graph->nodeCount(),
false);
1251 std::map<TNodeID, std::set<TNodeID> > neighbors_of;
1252 this->
m_graph->getAdjacencyMatrix(neighbors_of);
1256 std::set<path_t*> pool_of_paths;
1258 std::set<TNodeID> starting_node_neighbors(neighbors_of.at(starting_node));
1260 starting_node_neighbors.begin();
1261 n_it != starting_node_neighbors.end(); ++n_it) {
1263 path_t* path_between_neighbors =
1267 pool_of_paths.insert(path_between_neighbors);
1270 visited_nodes.at(starting_node) =
true;
1287 it != visited_nodes.end(); ++it) {
1296 if (visited_nodes.at(ending_node)) {
1328 if (!visited_nodes.at(dest)) {
1330 visited_nodes.at(dest)=
true;
1334 this->
addToPaths(&pool_of_paths, *optimal_path, neighbors_of.at(dest));
1343 template<
class GRAPH_T>
1345 std::set<path_t*>* pool_of_paths,
1346 const path_t& current_path,
1347 const std::set<mrpt::utils::TNodeID>& neighbors)
const {
1350 using namespace std;
1360 neigh_it != neighbors.end(); ++neigh_it) {
1361 if (*neigh_it == second_to_last_node)
continue;
1364 path_t path_between_nodes;
1366 &path_between_nodes);
1371 *path_to_append = current_path;
1372 *path_to_append += path_between_nodes;
1374 pool_of_paths->insert(path_to_append);
1380 template<
class GRAPH_T>
1383 using namespace std;
1390 path = search->second;
1398 template<
class GRAPH_T>
1402 path_t* path_between_nodes)
const {
1406 using namespace std;
1409 this->
m_graph->edgeExists(from, to) ||
1410 this->
m_graph->edgeExists(to, from),
1412 "(%lu <-> %lu) does not exist\n", from, to) );
1418 path_between_nodes->
clear();
1422 double curr_determinant = 0;
1424 std::pair<edges_citerator, edges_citerator> fwd_edges_pair =
1425 this->
m_graph->getEdges(from, to);
1434 edges_it != fwd_edges_pair.second; ++edges_it) {
1438 curr_edge.copyFrom(edges_it->second);
1442 curr_edge.getInformationMatrix(inf_mat);
1446 curr_edge.cov_inv = inf_mat;
1456 *path_between_nodes = curr_path;
1460 std::pair<edges_citerator, edges_citerator> bwd_edges_pair =
1461 this->
m_graph->getEdges(to, from);
1470 edges_it != bwd_edges_pair.second; ++edges_it) {
1474 (edges_it->second).
inverse(curr_edge);
1478 curr_edge.getInformationMatrix(inf_mat);
1482 curr_edge.cov_inv = inf_mat;
1492 *path_between_nodes = curr_path;
1499 template<
class GRAPH_T>
1502 typename std::set<path_t*>* pool_of_paths)
const {
1504 using namespace std;
1507 path_t* optimal_path = NULL;
1508 double curr_determinant = 0;
1510 it = pool_of_paths->begin();
1511 it != pool_of_paths->end();
1516 if (curr_determinant < (*it)->getDeterminant()) {
1523 pool_of_paths->erase(optimal_path);
1525 return optimal_path;
1529 template<
class GRAPH_T>
1535 using namespace std;
1542 (rel_edge.getMeanVal()-initial_estim).getAsVector(mean_diff);
1549 bool mahal_distance_null =
isNaN(mahal_distance);
1550 if (!mahal_distance_null) {
1557 bool accept_edge = (threshold >= mahal_distance && !mahal_distance_null) ?
true :
false;
1567 template<
class GRAPH_T>
1574 template<
class GRAPH_T>
1582 using namespace std;
1592 this->logFmt(LVL_INFO,
"\tLoop Closure edge!");
1599 this->
m_graph->insertEdge(from, to, rel_edge);
1604 template<
class GRAPH_T>
1617 "Toggle LaserScans Visualization");
1619 "Toggle Map Partitions Visualization");
1623 this->logFmt(LVL_DEBUG,
1624 "Fetched the window manager, window observer successfully.");
1628 template<
class GRAPH_T>
1630 const std::map<std::string, bool>& events_occurred) {
1646 template<
class GRAPH_T>
1648 using namespace mrpt;
1661 CSetOfObjectsPtr map_partitions_obj = CSetOfObjects::Create();
1662 map_partitions_obj->setName(
"map_partitions");
1665 scene->insert(map_partitions_obj);
1671 template<
class GRAPH_T>
1673 using namespace mrpt;
1682 std::stringstream title;
1695 CSetOfObjectsPtr map_partitions_obj;
1697 CRenderizablePtr obj = scene->getByName(
"map_partitions");
1699 map_partitions_obj =
static_cast<CSetOfObjectsPtr
>(obj);
1702 int partitionID = 0;
1703 bool partition_contains_last_node =
false;
1704 bool found_last_node =
false;
1706 << (this->
m_graph->nodeCount()-1));
1718 this->
m_graph->nodeCount()-1) != nodes_list.end()) {
1719 partition_contains_last_node =
true;
1721 found_last_node =
true;
1724 partition_contains_last_node =
false;
1728 std::string partition_obj_name =
mrpt::format(
"partition_%d", partitionID);
1729 std::string balloon_obj_name =
mrpt::format(
"#%d", partitionID);
1731 CRenderizablePtr obj = map_partitions_obj->getByName(partition_obj_name);
1732 CSetOfObjectsPtr curr_partition_obj;
1737 curr_partition_obj =
static_cast<CSetOfObjectsPtr
>(obj);
1739 curr_partition_obj->setVisibility(partition_contains_last_node);
1744 "\tCreating a new CSetOfObjects partition object for partition #" <<
1746 curr_partition_obj = CSetOfObjects::Create();
1747 curr_partition_obj->setName(partition_obj_name);
1750 curr_partition_obj->setVisibility(partition_contains_last_node);
1754 CSpherePtr balloon_obj = CSphere::Create();
1755 balloon_obj->setName(balloon_obj_name);
1758 balloon_obj->enableShowName();
1760 curr_partition_obj->insert(balloon_obj);
1765 CSetOfLinesPtr connecting_lines_obj = CSetOfLines::Create();
1766 connecting_lines_obj->setName(
"connecting_lines");
1768 connecting_lines_obj->setLineWidth(0.1f);
1770 curr_partition_obj->insert(connecting_lines_obj);
1774 map_partitions_obj->insert(curr_partition_obj);
1779 std::pair<double, double> centroid_coords;
1782 TPoint3D balloon_location(centroid_coords.first, centroid_coords.second,
1787 CSpherePtr balloon_obj;
1790 CRenderizablePtr obj = curr_partition_obj->getByName(balloon_obj_name);
1791 balloon_obj =
static_cast<CSpherePtr
>(obj);
1792 balloon_obj->setLocation(balloon_location);
1793 if (partition_contains_last_node)
1803 CSetOfLinesPtr connecting_lines_obj;
1806 CRenderizablePtr obj = curr_partition_obj->getByName(
"connecting_lines");
1807 connecting_lines_obj =
static_cast<CSetOfLinesPtr
>(obj);
1809 connecting_lines_obj->clear();
1812 it != nodes_list.end(); ++it) {
1814 TPoint3D curr_node_location(curr_pose);
1816 TSegment3D connecting_line(curr_node_location, balloon_location);
1817 connecting_lines_obj->appendLine(connecting_line);
1824 if (!found_last_node) {
1835 if (curr_size < prev_size) {
1837 for (
size_t partitionID = curr_size; partitionID != prev_size; ++partitionID) {
1841 static_cast<unsigned long>(partitionID));
1843 CRenderizablePtr obj = map_partitions_obj->getByName(partition_obj_name);
1846 <<
" was not found.");
1849 map_partitions_obj->removeObject(obj);
1859 template<
class GRAPH_T>
1867 this->logFmt(LVL_INFO,
"Toggling map partitions visualization...");
1871 mrpt::opengl::CRenderizablePtr obj = scene->getByName(
"map_partitions");
1872 obj->setVisibility(!obj->isVisible());
1884 template<
class GRAPH_T>
1887 std::pair<double, double>* centroid_coords)
const {
1892 double centroid_x = 0;
1893 double centroid_y = 0;
1895 node_it != nodes_list.end(); ++node_it) {
1897 centroid_x += curr_node_pos.x();
1898 centroid_y += curr_node_pos.y();
1903 centroid_coords->first = centroid_x/
static_cast<double>(nodes_list.size());
1904 centroid_coords->second = centroid_y/
static_cast<double>(nodes_list.size());
1909 template<
class GRAPH_T>
1918 mrpt::opengl::CPlanarLaserScanPtr laser_scan_viz =
1920 laser_scan_viz->enablePoints(
true);
1921 laser_scan_viz->enableLine(
true);
1922 laser_scan_viz->enableSurface(
true);
1923 laser_scan_viz->setSurfaceColor(
1929 laser_scan_viz->setName(
"laser_scan_viz");
1931 scene->insert(laser_scan_viz);
1939 template<
class GRAPH_T>
1947 mrpt::opengl::CRenderizablePtr obj = scene->getByName(
"laser_scan_viz");
1948 mrpt::opengl::CPlanarLaserScanPtr laser_scan_viz =
1949 static_cast<mrpt::opengl::CPlanarLaserScanPtr
>(obj);
1950 ASSERT_(laser_scan_viz.present());
1956 if (search != this->
m_graph->nodes.end()) {
1957 laser_scan_viz->setPose(search->second);
1961 laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(), -0.15,
1976 template<
class GRAPH_T>
1983 this->logFmt(LVL_INFO,
"Toggling LaserScans visualization...");
1988 mrpt::opengl::CRenderizablePtr obj = scene->getByName(
"laser_scan_viz");
1989 obj->setVisibility(!obj->isVisible());
2002 template<
class GRAPH_T>
2004 std::map<std::string, int>* edge_types_to_num)
const {
2010 template<
class GRAPH_T>
2018 "Configuration parameters aren't loaded yet");
2033 template<
class GRAPH_T>
2054 template<
class GRAPH_T>
2057 using namespace std;
2065 std::string title(
"Position uncertainty");
2073 CEllipsoidPtr cov_ellipsis_obj = CEllipsoid::Create();
2074 cov_ellipsis_obj->setName(
"cov_ellipsis_obj");
2076 cov_ellipsis_obj->setLocation(0, 0, 0);
2080 scene->insert(cov_ellipsis_obj);
2087 template<
class GRAPH_T>
2090 using namespace std;
2106 stringstream ss_mat; ss_mat << mat;
2107 this->logFmt(LVL_DEBUG,
"In updateCurrCovarianceVisualization\n" 2108 "Covariance matrix:\n%s\n" 2109 "determinant : %f", ss_mat.str().c_str(), mat.det() );
2112 CRenderizablePtr obj = scene->getByName(
"cov_ellipsis_obj");
2113 CEllipsoidPtr cov_ellipsis_obj =
static_cast<CEllipsoidPtr
>(obj);
2116 cov_ellipsis_obj->setLocation(curr_position.x(), curr_position.y(), 0);
2119 cov_ellipsis_obj->setCovMatrix(mat, 2);
2127 template<
class GRAPH_T>
2129 std::string viz_flag,
int sleep_time ) {
2132 this->logFmt(mrpt::utils::LVL_ERROR,
2133 "Cannot toggle visibility of specified object.\n " 2134 "Make sure that the corresponding visualization flag ( %s " 2135 ") is set to true in the .ini file.\n",
2143 template<
class GRAPH_T>
2149 "EdgeRegistrationDeciderParameters");
2151 "EdgeRegistrationDeciderParameters");
2153 "EdgeRegistrationDeciderParameters");
2158 "EdgeRegistrationDeciderParameters",
2159 "consec_icp_constraint_factor",
2162 "EdgeRegistrationDeciderParameters",
2163 "lc_icp_constraint_factor",
2168 int min_verbosity_level = source.
read_int(
2169 "EdgeRegistrationDeciderParameters",
2174 this->setMinLoggingLevel(mrpt::utils::VerbosityLevel(min_verbosity_level));
2177 template<
class GRAPH_T>
2180 using namespace std;
2182 cout <<
"------------------[Pair-wise Consistency of ICP Edges - Registration Procedure Summary]------------------" << endl;
2196 template<
class GRAPH_T>
2201 std::stringstream class_props_ss;
2203 "Pair-wise Consistency of ICP Edges - Registration Procedure Summary: " <<
2205 class_props_ss << this->
header_sep << std::endl;
2209 const std::string output_res = this->getLogAsString();
2212 report_str->clear();
2215 *report_str += class_props_ss.str();
2218 *report_str += time_res;
2221 *report_str += output_res;
2227 template<
class GRAPH_T>
2233 template<
class GRAPH_T>
2234 const std::vector<mrpt::vector_uint>&
2239 template<
class GRAPH_T>
2242 bool is_first_time_node_reg ) {
2246 using namespace std;
2253 this->logFmt(LVL_INFO,
2254 "updateMapPartitions: Full partitioning of map was issued");
2262 if (is_first_time_node_reg) {
2263 nodes_to_scans.insert(
2264 make_pair(this->
m_graph->root,
2265 this->m_nodes_to_laser_scans2D.at(this->m_graph->root)));
2269 nodes_to_scans.insert(
2279 it = nodes_to_scans.begin();
2280 it != nodes_to_scans.end(); ++it) {
2281 if ((it->second).null()) {
2283 "nodeID \"" << it->first <<
"\" has invalid laserScan");
2290 search = this->
m_graph->nodes.find(it->first);
2291 if (search == this->
m_graph->nodes.end()) {
2297 pose_t curr_pose = search->second;
2298 mrpt::poses::CPosePDFPtr posePDF(
new constraint_t(curr_pose));
2302 sf->insert(it->second);
2311 for (
size_t i = 0; i < curr_size; i++) {
2325 template<
class GRAPH_T>
2327 laser_scans_color(0, 20, 255),
2328 keystroke_laser_scans(
"l"),
2329 has_read_config(false)
2335 template<
class GRAPH_T>
2338 template<
class GRAPH_T>
2343 out.
printf(
"Use scan-matching constraints = %s\n",
2345 out.
printf(
"Num. of previous nodes to check ICP against = %d\n",
2347 out.
printf(
"Visualize laser scans = %s\n",
2352 template<
class GRAPH_T>
2355 const std::string& section) {
2360 "use_scan_matching",
2364 "prev_nodes_for_ICP",
2367 "VisualizationParameters",
2368 "visualize_laser_scans",
2379 template<
class GRAPH_T>
2381 keystroke_map_partitions(
"b"),
2382 balloon_elevation(3),
2383 balloon_radius(0.5),
2384 balloon_std_color(153, 0, 153),
2385 balloon_curr_color(62, 0, 80),
2386 connecting_lines_color(balloon_std_color),
2391 template<
class GRAPH_T>
2394 template<
class GRAPH_T>
2398 using namespace std;
2401 ss <<
"Min. node difference for loop closure = " <<
2403 ss <<
"Remote NodeIDs to consider the potential loop closure = " <<
2405 ss <<
"Min EigenValues ratio for accepting a hypotheses set = " <<
2407 ss <<
"Check only current node's partition for loop closures = " <<
2409 ss <<
"New registered nodes required for full partitioning = " <<
2411 ss <<
"Visualize map partitions = " <<
2414 out.
printf(
"%s", ss.str().c_str());
2418 template<
class GRAPH_T>
2421 const std::string& section) {
2425 "GeneralConfiguration",
2426 "LC_min_nodeid_diff",
2430 "LC_min_remote_nodes",
2434 "LC_eigenvalues_ratio_thresh",
2438 "LC_check_curr_partition_only",
2442 "full_partition_per_nodes",
2445 "VisualizationParameters",
2446 "visualize_map_partitions",
int m_text_index_curr_node_covariance
void toggleMapPartitionsVisualization()
Toggle the map partitions visualization objects.
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
partitions_t m_curr_partitions
Current partitions vector.
void registerKeystroke(const std::string key_str, const std::string key_desc)
Make new keystrokes available in the help message box.
std::vector< mrpt::utils::TNodeID > nodes_traversed
Nodes that the Path comprises of.
mrpt::slam::CIncrementalMapPartitioner m_partitioner
Instance responsible for partitioning the map.
bool mahalanobisDistanceOdometryToICPEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge)
brief Compare the suggested ICP edge against the initial node difference.
void forceRepaint()
Repaints the window. forceRepaint, repaint and updateWindow are all aliases of the same method...
GRAPH_T::global_pose_t pose
virtual void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
double goodness
Goodness value corresponding to the hypothesis edge.
virtual void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
parent_t::nodes_to_scans2D_t nodes_to_scans2D_t
Holds the data of an information path.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
const mrpt::utils::TColor m_curr_node_covariance_color
void addToPath(const mrpt::utils::TNodeID &node, const constraint_t &edge)
add a new link in the current path.
const mrpt::utils::TColor laser_scans_color
see Constructor for initialization
static const path_t * findPathByEnds(const paths_t &vec_paths, const mrpt::utils::TNodeID &src, const mrpt::utils::TNodeID &dst, bool throw_exc=true)
Given a vector of TUncertaintyPath objects, find the one that has the given source and destination no...
int text_index_map_partitions
std::vector< uint32_t > vector_uint
mrpt::gui::CDisplayWindow3D * m_win
Window to use.
double DEG2RAD(const double x)
Degrees to radians.
mrpt::obs::CObservation2DRangeScanPtr m_first_laser_scan
Keep track of the first recorded laser scan so that it can be assigned to the root node when the NRD ...
void initMapPartitionsVisualization()
Initialize the visualization of the map partition objects.
mrpt::graphs::detail::THypothesis< GRAPH_T > hypot_t
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
void computeCentroidOfNodesVector(const vector_uint &nodes_list, std::pair< double, double > *centroid_coords) const
Compute the Centroid of a group of a vector of node positions.
void getEdge(constraint_t *edge) const
Getter methods for the underlying edge.
void updateCurrCovarianceVisualization()
bool m_partitions_full_update
Indicate whether the partitions have been updated recently.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define THROW_EXCEPTION(msg)
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
Keep the last laser scan for visualization purposes.
static const std::string header_sep
Separator string to be used in debugging messages.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
std::string getSTLContainerAsString(const T &t)
Return a STL container in std::string form.
void resizeWindow(size_t new_size)
Resize the window.
mrpt::graphslam::CWindowObserver * m_win_observer
CWindowObserver object for monitoring various visual-oriented events.
std::string keystroke_laser_scans
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
T value(details::expression_node< T > *n)
size_t m_last_total_num_nodes
Keep track of the total number of registered nodes since the last time class method was called...
bool use_scan_matching
Indicate whethet to use scan-matching at all during graphSLAM [on by default].
const Scalar * const_iterator
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
pose_t init_estim
Initial ICP estimation.
const_iterator find(const KEY &key) const
TSlidingWindow mahal_distance_ICP_odom_win
Keep track of the mahalanobis distance between the initial pose difference and the suggested new edge...
void addTextMessage(const double x, const double y, const std::string &text, const mrpt::utils::TColorf &color=mrpt::utils::TColorf(1.0, 1.0, 1.0), const size_t unique_index=0)
Wrapper around the CDisplayWindow3D::addTextMessage method, so that the user does not have to specify...
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
static hypot_t * findHypotByID(const hypotsp_t &vec_hypots, const size_t &id, bool throw_exc=true)
Given a vector of THypothesis objects, find the one that has the given ID.
GRAPH_T::edges_map_t::const_iterator edges_citerator
virtual bool getICPEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, constraint_t *rel_edge, mrpt::slam::CICP::TReturnInfo *icp_info=NULL, const TGetICPEdgeAdParams *ad_params=NULL)
Get the ICP Edge between the provided nodes.
virtual ~CLoopCloserERD()
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
node_props_t from_params
Ad.
Struct for passing additional parameters to the getICPEdge call.
void addToPaths(std::set< path_t *> *pool_of_paths, const path_t &curr_path, const std::set< mrpt::utils::TNodeID > &neibors) const
Append the paths starting from the current node.
void registerHypothesis(const hypot_t &h)
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
void getAsString(std::string *str) const
std::map< mrpt::utils::TNodeID, path_t *> m_node_optimal_paths
Map that stores the lowest uncertainty path towards a node.
void evaluatePartitionsForLC(const partitions_t &partitions)
Evaluate the given partitions for loop closures.
TLoopClosureParams m_lc_params
mrpt::utils::TNodeID from
Starting node of the hypothesis.
This class allows loading and storing values and vectors of different types from a configuration text...
bool m_visualize_curr_node_covariance
bool is_valid
Field that specifies if the hypothesis is to be considered.
mrpt::graphslam::CWindowManager * m_win_manager
Pointer to the CWindowManager object used to store visuals-related instances.
double getMedian()
Return the current median value.
bool visualize_laser_scans
double LC_eigenvalues_ratio_thresh
Eigenvalues ratio for accepting/rejecting a hypothesis set.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
SLAM methods related to graphs of pose constraints.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
3D segment, consisting of two points.
uint64_t TNodeID
The type for node IDs in graphs of different types.
virtual void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
bool m_override_registered_nodes_check
Indicates whether the ERD implementation expects, at most one single node to be registered, between successive calls to the updateState method.
constraint_t curr_pose_pdf
Current path position + corresponding covariance.
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void addNewMeasurement(double measurement)
Update the sliding window by appending a new measurement.
virtual void addScanMatchingEdges(const mrpt::utils::TNodeID &curr_nodeID)
Addd ICP constraints from X previous nodeIDs up to the given nodeID.
const mrpt::utils::TColor balloon_curr_color
std::string keystroke_map_partitions
std::map< int, vector_uint > m_partitionID_to_prev_nodes_list
Keep track of the evaluated partitions so they are not checked again if nothing changed in them...
TLaserParams m_laser_params
virtual void fetchNodeIDsForScanMatching(const mrpt::utils::TNodeID &curr_nodeID, std::set< mrpt::utils::TNodeID > *nodes_set)
Fetch a list of nodes with regards prior to the given nodeID for which to try and add scan matching e...
static const std::string report_sep
const mrpt::utils::TColor connecting_lines_color
virtual void registerNewEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
This namespace contains representation of robot actions and observations.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
mrpt::slam::CIncrementalMapPartitioner::TOptions options
const partitions_t & getCurrPartitions() const
int prev_nodes_for_ICP
How many nodes back to check ICP against?
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
bool m_is_first_time_node_reg
Track the first node registration occurance.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
uint64_t TNodeID
The type for node IDs in graphs of different types.
std::map< std::string, int > m_edge_types_to_nums
Keep track of the registered edge types.
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager)
Fetch a CWindowManager pointer.
GRAPH_T * m_graph
Pointer to the graph that is under construction.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
int full_partition_per_nodes
Full partition of map only afer X new nodes have been registered.
std::vector< path_t > paths_t
const double balloon_radius
unsigned int addMapFrame(const mrpt::obs::CSensoryFramePtr &frame, const mrpt::poses::CPosePDFPtr &robotPose2D)
bool computeDominantEigenVector(const mrpt::math::CMatrixDouble &consist_matrix, mrpt::math::dynamic_vector< double > *eigvec, bool use_power_method=false)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
MAT::Scalar mahalanobisDistance2(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance inverse ...
void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager)
Fetch a CWindowManager pointer.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
GRAPH_T::global_pose_t global_pose_t
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
void assertIsBetweenNodeIDs(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to) const
Assert that the current path is between the given nodeIDs.
void evalPWConsistenciesMatrix(const mrpt::math::CMatrixDouble &consist_matrix, const hypotsp_t &hypots_pool, hypotsp_t *valid_hypots)
Evalute the consistencies matrix, fill the valid hypotheses.
void getInverseEdge(constraint_t *edge) const
Getter methods for the inverse of the underlying edge.
double generatePWConsistencyElement(const mrpt::utils::TNodeID &a1, const mrpt::utils::TNodeID &a2, const mrpt::utils::TNodeID &b1, const mrpt::utils::TNodeID &b2, const hypotsp_t &hypots, const paths_t *opt_paths=NULL)
Return the pair-wise consistency between the observations of the given nodes.
bool LC_check_curr_partition_only
flag indicating whether to check only the partition of the last registered node for potential loop cl...
std::vector< mrpt::vector_uint > partitions_t
Typedef for referring to a list of partitions.
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
std::vector< hypot_t * > hypotsp_t
void registerNewEdge(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
void loadFromConfigFileName(const std::string &config_file, const std::string §ion)
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile ob...
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void splitPartitionToGroups(vector_uint &partition, vector_uint *groupA, vector_uint *groupB, int max_nodes_in_group=5)
Split an existing partition to Groups.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
static CPlanarLaserScanPtr Create()
const double balloon_elevation
mrpt::utils::TNodeID to
Ending node of the hypothesis.
node_props_t to_params
Ad.
void updateLaserScansVisualization()
virtual void updateVisuals()
Update the relevant visual features in CDisplayWindow.
virtual bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
void checkPartitionsForLC(partitions_t *partitions_for_LC)
Check the registered so far partitions for potential loop closures.
Struct for passing additional parameters to the generateHypotsPool call.
The ICP algorithm return information.
void setEdge(const constraint_t &edge)
Setter method for the underlying edge.
void generatePWConsistenciesMatrix(const vector_uint &groupA, const vector_uint &groupB, const hypotsp_t &hypots_pool, mrpt::math::CMatrixDouble *consist_matrix, const paths_t *groupA_opt_paths=NULL, const paths_t *groupB_opt_paths=NULL)
Compute the pair-wise consistencies Matrix.
double m_lc_icp_constraint_factor
Factor used for accepting an ICP Constraint in the loop closure proc.
static hypot_t * findHypotByEnds(const hypotsp_t &vec_hypots, const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, bool throw_exc=true)
Given a vector of THypothesis objects, find the one that has the given start and end nodes...
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
double m_offset_y_curr_node_covariance
The namespace for 3D scene representation and rendering.
A RGB color - floats in the range [0,1].
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
void updateMapPartitions(bool full_update=false, bool is_first_time_node_reg=false)
Split the currently registered graph nodes into partitions.
double leave(const char *func_name)
End of a named section.
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
int LC_min_remote_nodes
how many remote nodes (large nodID difference should there be before I consider the potential loop cl...
void getMinUncertaintyPath(const mrpt::utils::TNodeID from, const mrpt::utils::TNodeID to, path_t *path) const
Given two nodeIDs compute and return the path connecting them.
std::map< mrpt::utils::TNodeID, node_props_t > group_t
void generateHypotsPool(const vector_uint &groupA, const vector_uint &groupB, hypotsp_t *generated_hypots, const TGenerateHypotsPoolAdParams *ad_params=NULL)
Generate the hypothesis pool for all the inter-group constraints between two groups of nodes...
Classes for creating GUI windows for 2D and 3D visualization.
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * popMinUncertaintyPath(std::set< path_t *> *pool_of_paths) const
Find the minimum uncertainty path from te given pool of TUncertaintyPath instances.
void initCurrCovarianceVisualization()
void execDijkstraProjection(mrpt::utils::TNodeID starting_node=0, mrpt::utils::TNodeID ending_node=INVALID_NODEID)
compute the minimum uncertainty of each node position with regards to the graph root.
void updateMapPartitionsVisualization()
Update the map partitions visualization.
static CSensoryFramePtr Create()
void initLaserScansVisualization()
size_t id
ID of the current hypothesis.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
size_t LC_min_nodeid_diff
nodeID difference for detecting potential loop closure in a partition.
An edge hypothesis between two nodeIDs.
const mrpt::utils::TColor balloon_std_color
double m_consec_icp_constraint_factor
Factor used for accepting an ICP Constraint as valid.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
mrpt::graphslam::TUncertaintyPath< GRAPH_T > path_t
size_t m_dijkstra_node_count_thresh
Node Count lower bound before executing dijkstra.
double offset_y_map_partitions
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
void enter(const char *func_name)
Start of a named section.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define ASSERTMSG_(f, __ERROR_MSG)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const
Fill the given map with the type of registered edges as well as the corresponding number of registrat...
Internal auxiliary classes.
bool visualize_map_partitions
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
nodes_to_scans2D_t m_nodes_to_laser_scans2D
Map for keeping track of the observation recorded at each graph position.
bool fillNodePropsFromGroupParams(const mrpt::utils::TNodeID &nodeID, const std::map< mrpt::utils::TNodeID, node_props_t > &group_params, node_props_t *node_props)
Fill the TNodeProps instance using the parameters from the map.
mrpt::obs::CObservation2DRangeScanPtr scan
const mrpt::utils::TNodeID & getDestination() const
Return the Destination node of this path.
partitions_t m_last_partitions
Previous partitions vector.
bool getPropsOfNodeID(const mrpt::utils::TNodeID &nodeID, global_pose_t *pose, mrpt::obs::CObservation2DRangeScanPtr &scan, const node_props_t *node_props=NULL) const
Fill the pose and LaserScan for the given nodeID.
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
bool approximatelyEqual(T1 a, T1 b, T2 epsilon)
Compare 2 floats and determine whether they are equal.
T hypot(const T v0, const T v1)
GRAPH_T::constraint_t constraint_t
Handy typedefs.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
std::string getAsString(bool oneline=true) const
Return a string representation of the object at hand.
void assignTextMessageParameters(double *offset_y, int *text_index)
Assign the next available offset_y and text_index for the textMessage under construction.
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * queryOptimalPath(const mrpt::utils::TNodeID node) const
Query for the optimal path of a nodeID.
TSlidingWindow goodness_threshold_win
Keep track of ICP Goodness values for ICP between nearby nodes and adapt the Goodness threshold based...