10 #ifndef CGRAPHSLAMENGINE_IMPL_H 11 #define CGRAPHSLAMENGINE_IMPL_H 13 namespace mrpt {
namespace graphslam {
15 template<
class GRAPH_T>
17 template<
class GRAPH_T>
23 template<
class GRAPH_T>
25 const std::string& config_file,
26 const std::string& rawlog_fname,
27 const std::string& fname_GT ,
35 m_optimizer(optimizer),
36 m_enable_visuals(win_manager != NULL),
37 m_config_fname(config_file),
38 m_rawlog_fname(rawlog_fname),
41 m_win_manager(win_manager),
42 m_paused_message(
"Program is paused. Press \"p/P\" to resume."),
43 m_text_index_paused_message(345),
44 m_odometry_color(0, 0, 255),
45 m_GT_color(0, 255, 0),
46 m_estimated_traj_color(255, 165, 0),
47 m_optimized_map_color(255, 0, 0),
48 m_current_constraint_type_color(255, 255, 255),
49 m_robot_model_size(1),
50 m_graph_section(
"graph_sec"),
51 m_class_name(
"CGraphSlamEngine"),
52 m_is_first_time_node_reg(true)
57 template<
class GRAPH_T>
68 if ((it->second)->fileOpenCorrectly()) {
70 (it->second)->close();
92 template<
class GRAPH_T>
93 typename GRAPH_T::global_pose_t
98 return m_node_reg->getCurrentRobotPosEstimation();
103 template<
class GRAPH_T>
105 typename GRAPH_T::global_poses_t* graph_poses)
const {
113 template<
class GRAPH_T>
115 std::set<mrpt::utils::TNodeID>* nodes_set)
const {
118 m_graph.getAllNodes(*nodes_set);
123 template<
class GRAPH_T>
126 using namespace mrpt;
133 this->logging_enable_keep_record =
true;
150 const string c_str(c.GetRuntimeClass()->className);
162 "Given graph class " << c_str <<
163 " has not been tested consistently yet." <<
164 "Proceed at your own risk.");
249 "Ground truth file was not provided. Switching the related visualization parameters off...");
308 CAxisPtr obj = CAxis::Create();
309 obj->setFrequency(5);
310 obj->enableTickMarks();
311 obj->setAxisLimits(-10,-10,-10, 10,10,10);
312 obj->setName(
"axis");
331 "Pause/Resume program execution");
333 "Toggle Odometry visualization");
335 "Toggle Ground-Truth visualization");
337 "Toggle Estimated trajectory visualization");
339 "Toggle Map visualization");
343 vector<string> vec_edge_types;
344 vec_edge_types.push_back(
"Odometry");
345 vec_edge_types.push_back(
"ICP2D");
346 vec_edge_types.push_back(
"ICP3D");
349 cit != vec_edge_types.end(); ++cit) {
356 double offset_y_total_edges, offset_y_loop_closures;
357 int text_index_total_edges, text_index_loop_closures;
360 &offset_y_total_edges,
361 &text_index_total_edges);
365 map<string, double> name_to_offset_y;
366 map<string, int> name_to_text_index;
368 it != vec_edge_types.end();
371 &name_to_offset_y[*it],
372 &name_to_text_index[*it]);
376 &offset_y_loop_closures,
377 &text_index_loop_closures);
381 name_to_offset_y, name_to_text_index,
382 offset_y_total_edges, text_index_total_edges,
383 offset_y_loop_closures, text_index_loop_closures);
424 gridmap->insertionOptions.maxOccupancyUpdateCertainty = 0.8f;
425 gridmap->insertionOptions.maxDistanceInsertion = 5;
426 gridmap->insertionOptions.wideningBeamsWithDistance =
true;
427 gridmap->insertionOptions.decimation = 2;
439 octomap->insertionOptions.setOccupancyThres(0.5);
440 octomap->insertionOptions.setProbHit(0.7);
441 octomap->insertionOptions.setProbMiss(0.4);
442 octomap->insertionOptions.setClampingThresMin(0.1192);
443 octomap->insertionOptions.setClampingThresMax(0.971);
455 int num_of_objects = std::atoi(
462 catch (std::exception&
e) {
484 template<
class GRAPH_T>
486 mrpt::obs::CObservationPtr& observation,
487 size_t& rawlog_entry) {
490 CActionCollectionPtr action;
491 CSensoryFramePtr observations;
497 template<
class GRAPH_T>
499 mrpt::obs::CActionCollectionPtr& action,
500 mrpt::obs::CSensoryFramePtr& observations,
501 mrpt::obs::CObservationPtr& observation,
502 size_t& rawlog_entry) {
506 using namespace mrpt;
517 format(
"\nConfig file is not read yet.\nExiting... \n"));
525 if (observation.present()) {
536 action->getFirstMovementEstimationMean(increment_pose_3d);
537 pose_t increment_pose(increment_pose_3d);
546 bool registered_new_node;
550 registered_new_node =
m_node_reg->updateState(
551 action, observations, observation);
556 CObservation2DRangeScanPtr scan =
557 getObservation<CObservation2DRangeScan>(observations, observation);
558 if (scan.present()) {
567 if (registered_new_node) {
574 if (
m_graph.nodeCount() != 2) {
576 <<
" but got [" <<
m_graph.nodeCount() <<
"]");
592 "NodeRegistrationDecider");
608 "EdgeRegistrationDecider");
621 "GraphSlamOptimizer");
626 if (observation.present()) {
632 CObservationOdometryPtr obs_odometry =
633 static_cast<CObservationOdometryPtr
>(observation);
640 static_cast<mrpt::obs::CObservation3DRangeScanPtr
>(observation);
647 ASSERT_(observations.present());
651 action->getFirstMovementEstimationMean(increment_pose_3d);
652 pose_t increment_pose(increment_pose_3d);
657 if (registered_new_node) {
667 bool full_update =
true;
677 std::map<std::string, int> edge_types_to_nums;
678 m_edge_reg->getEdgesStats(&edge_types_to_nums);
679 if (edge_types_to_nums.size()) {
681 edge_types_to_nums.begin(); it != edge_types_to_nums.end();
701 bool full_update =
true;
763 if (rawlog_entry % 2 == 0) {
806 template<
class GRAPH_T>
811 m_graph.dijkstra_nodes_estimate();
817 template<
class GRAPH_T>
820 std::string class_name) {
823 using namespace mrpt;
831 format(
"listed_nodeCount [%lu] != nodeCount() [%lu]",
832 static_cast<unsigned long>(listed_nodeCount),
833 static_cast<unsigned long>(
m_graph.nodeCount())));
836 if (listed_nodeCount !=
m_graph.nodeCount()) {
838 " illegally added new nodes to the graph " <<
839 ", wanted to see [" << listed_nodeCount <<
"] but saw [" 840 <<
m_graph.nodeCount() <<
"]");
842 class_name.c_str()));
848 template<
class GRAPH_T>
850 mrpt::maps::COccupancyGridMap2DPtr map,
865 if (acquisition_time) {
871 template<
class GRAPH_T>
873 mrpt::maps::COctoMapPtr map,
885 if (acquisition_time) {
892 template<
class GRAPH_T>
896 using namespace mrpt;
903 if (!constraint_t::is_3D()) {
915 const TNodeID& curr_node = it->first;
918 const mrpt::obs::CObservation2DRangeScanPtr& curr_laser_scan = it->second;
920 format(
"LaserScan of nodeID: %lu is not present.",
921 static_cast<unsigned long>(curr_node)));
927 gridmap->insertObservation(curr_laser_scan.pointer(), &scan_pose);
935 THROW_EXCEPTION(
"Not Implemented Yet. Method is to compute a COctoMap");
943 template<
class GRAPH_T>
945 const std::string& fname) {
950 mrpt::format(
"\nConfiguration file not found: \n%s\n", fname.c_str()));
959 "GeneralConfiguration",
960 "user_decides_about_output_dir",
963 "GeneralConfiguration",
964 "ground_truth_file_format",
968 int min_verbosity_level = cfg_file.
read_int(
969 "GeneralConfiguration",
972 this->setMinLoggingLevel(VerbosityLevel(min_verbosity_level));
979 "VisualizationParameters",
985 "VisualizationParameters",
986 "visualize_odometry_poses",
989 "VisualizationParameters",
990 "visualize_estimated_trajectory",
995 "VisualizationParameters",
996 "visualize_ground_truth",
1000 "VisualizationParameters",
1001 "visualize_SLAM_metric",
1006 "VisualizationParameters",
1007 "enable_curr_pos_viewport",
1010 "VisualizationParameters",
1011 "enable_range_viewport",
1014 "VisualizationParameters",
1015 "enable_intensity_viewport",
1025 template<
class GRAPH_T>
1035 template<
class GRAPH_T>
1037 std::string* params_out)
const {
1040 using namespace std;
1042 stringstream ss_out;
1044 ss_out <<
"\n------------[ Graphslam_engine Problem Parameters ]------------" 1046 ss_out <<
"Config filename = " 1049 ss_out <<
"Ground Truth File format = " 1051 ss_out <<
"Ground Truth filename = " 1054 ss_out <<
"Visualize odometry = " 1056 ss_out <<
"Visualize estimated trajectory = " 1058 ss_out <<
"Visualize map = " 1060 ss_out <<
"Visualize Ground Truth = " 1063 ss_out <<
"Visualize SLAM metric plot = " 1066 ss_out <<
"Enable curr. position viewport = " 1068 ss_out <<
"Enable range img viewport = " 1070 ss_out <<
"Enable intensity img viewport = " 1073 ss_out <<
"-----------------------------------------------------------" 1075 ss_out << std::endl;
1078 *params_out = ss_out.str();
1083 template<
class GRAPH_T>
1095 template<
class GRAPH_T>
1097 const std::string& fname) {
1100 using namespace std;
1106 #if defined(MRPT_OS_APPLE) 1108 std::string time_spec =
"UTC Time";
1111 std::string time_spec =
"Time";
1119 "\nError while trying to open %s\n", fname.c_str()) );
1121 const std::string sep(80,
'#');
1123 m_out_streams[fname]->printf(
"# Mobile Robot Programming Toolkit (MRPT)\n");
1125 m_out_streams[fname]->printf(
"# GraphSlamEngine Application\n");
1126 m_out_streams[fname]->printf(
"# Automatically generated file - %s: %s\n",
1128 cur_date_str.c_str());
1134 template<
class GRAPH_T>
1141 COpenGLViewportPtr viewp_range;
1143 viewp_range = scene->createViewport(
"viewp_range");
1146 viewp_range->setViewportPosition(x, y, h, w);
1154 template<
class GRAPH_T>
1168 template<
class GRAPH_T>
1187 COpenGLViewportPtr viewp_range = scene->getViewport(
"viewp_range");
1188 viewp_range->setImageView(img);
1197 template<
class GRAPH_T>
1204 COpenGLViewportPtr viewp_intensity;
1206 viewp_intensity = scene->createViewport(
"viewp_intensity");
1209 viewp_intensity->setViewportPosition(x, y, w, h);
1216 template<
class GRAPH_T>
1230 COpenGLViewportPtr viewp_intensity = scene->getViewport(
"viewp_intensity");
1231 viewp_intensity->setImageView(img);
1240 template<
class GRAPH_T>
1246 template<
class GRAPH_T>
1253 template<
class GRAPH_T>
1260 template<
class GRAPH_T>
1268 COpenGLViewportPtr viewp= scene->createViewport(
"curr_robot_pose_viewport");
1270 viewp->setCloneView(
"main");
1273 viewp->setViewportPosition(x, y, h, w);
1274 viewp->setTransparent(
false);
1275 viewp->getCamera().setAzimuthDegrees(90);
1276 viewp->getCamera().setElevationDegrees(90);
1277 viewp->setCustomBackgroundColor(
TColorf(205, 193, 197, 255));
1278 viewp->getCamera().setZoomDistance(30);
1279 viewp->getCamera().setOrthogonal();
1289 template<
class GRAPH_T>
1302 COpenGLViewportPtr viewp = scene->getViewport(
"curr_robot_pose_viewport");
1303 viewp->getCamera().setPointingAt(
CPose3D(curr_robot_pose));
1312 template<
class GRAPH_T>
1314 const std::string& fname_GT,
1315 std::vector<mrpt::poses::CPose2D>* gt_poses,
1316 std::vector<mrpt::system::TTimeStamp>* gt_timestamps ) {
1318 using namespace std;
1324 format(
"\nGround-truth file %s was not found.\n" 1325 "Either specify a valid ground-truth filename or set set the " 1326 "m_visualize_GT flag to false\n", fname_GT.c_str()));
1330 ASSERTMSG_(gt_poses,
"\nNo valid std::vector<pose_t>* was given\n");
1335 for (
size_t line_num = 0; file_GT.
readLine(curr_line); line_num++) {
1336 vector<string> curr_tokens;
1340 ASSERTMSG_(curr_tokens.size() == constraint_t::state_length + 1,
1341 "\nGround Truth File doesn't seem to contain data as generated by the " 1342 "GridMapNavSimul application.\n Either specify the GT file format or set " 1343 "visualize_ground_truth to false in the .ini file\n");
1346 if (gt_timestamps) {
1348 gt_timestamps->push_back(timestamp);
1353 atof(curr_tokens[1].c_str()),
1354 atof(curr_tokens[2].c_str()),
1355 atof(curr_tokens[3].c_str()) );
1356 gt_poses->push_back(curr_pose);
1363 template<
class GRAPH_T>
1365 const std::string& fname_GT,
1366 std::vector<mrpt::poses::CPose3D>* gt_poses,
1367 std::vector<mrpt::system::TTimeStamp>* gt_timestamps ) {
1371 template<
class GRAPH_T>
1373 const std::string& fname_GT,
1374 std::vector<mrpt::poses::CPose2D>* gt_poses,
1375 std::vector<mrpt::system::TTimeStamp>* gt_timestamps) {
1377 using namespace std;
1384 format(
"\nGround-truth file %s was not found.\n" 1385 "Either specify a valid ground-truth filename or set set the " 1386 "m_visualize_GT flag to false\n", fname_GT.c_str()));
1390 "\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
1391 ASSERTMSG_(gt_poses,
"No valid std::vector<pose_t>* was given");
1396 for (
size_t i = 0; file_GT.
readLine(curr_line) ; i++) {
1397 if (curr_line.at(0) !=
'#') {
1405 vector<string> curr_tokens;
1410 "\nGround Truth File doesn't seem to contain data as specified in RGBD_TUM related " 1411 "datasets.\n Either specify correct the GT file format or set " 1412 "visualize_ground_truth to false in the .ini file\n");
1416 quat.
r(atof(curr_tokens[7].c_str()));
1417 quat.x(atof(curr_tokens[4].c_str()));
1418 quat.y(atof(curr_tokens[5].c_str()));
1419 quat.z(atof(curr_tokens[6].c_str()));
1424 curr_coords[0] = atof(curr_tokens[1].c_str());
1425 curr_coords[1] = atof(curr_tokens[2].c_str());
1426 curr_coords[2] = atof(curr_tokens[3].c_str());
1435 pose_diff = curr_pose;
1438 for (; file_GT.
readLine(curr_line) ;) {
1439 vector<string> curr_tokens;
1442 "\nGround Truth File doesn't seem to contain data as specified in RGBD_TUM related " 1443 "datasets.\n Either specify correct the GT file format or set " 1444 "visualize_ground_truth to false in the .ini file\n");
1447 if (gt_timestamps) {
1449 gt_timestamps->push_back(timestamp);
1454 quat.
r(atof(curr_tokens[7].c_str()));
1455 quat.
x(atof(curr_tokens[4].c_str()));
1456 quat.
y(atof(curr_tokens[5].c_str()));
1457 quat.
z(atof(curr_tokens[6].c_str()));
1462 curr_coords[0] = atof(curr_tokens[1].c_str());
1463 curr_coords[1] = atof(curr_tokens[2].c_str());
1464 curr_coords[2] = atof(curr_tokens[3].c_str());
1472 curr_pose.x() -= pose_diff.x();
1473 curr_pose.y() -= pose_diff.y();
1474 curr_pose.phi() -= pose_diff.phi();
1476 gt_poses->push_back(curr_pose);
1484 template<
class GRAPH_T>
1487 using namespace std;
1498 const double tmpz[] = {
1499 cos(anglez), -sin(anglez), 0,
1500 sin(anglez), cos(anglez), 0,
1507 const double tmpy[] = {
1508 cos(angley), 0, sin(angley),
1510 -sin(angley), 0, cos(angley) };
1516 const double tmpx[] = {
1518 0, cos(anglex), -sin(anglex),
1519 0, sin(anglex), cos(anglex) };
1522 stringstream ss_out;
1523 ss_out <<
"\nConstructing the rotation matrix for the GroundTruth Data..." 1527 ss_out <<
"Rotation matrices for optical=>MRPT transformation" << endl;
1528 ss_out <<
"rotz: " << endl << rotz << endl;
1529 ss_out <<
"roty: " << endl << roty << endl;
1530 ss_out <<
"rotx: " << endl << rotx << endl;
1538 template<
class GRAPH_T>
1543 "\nqueryObserverForEvents method was called even though no Observer object was provided\n");
1546 std::map<std::string, bool> events_occurred;
1573 m_node_reg->notifyOfWindowEvents(events_occurred);
1574 m_edge_reg->notifyOfWindowEvents(events_occurred);
1580 template<
class GRAPH_T>
1591 CRenderizablePtr obj = scene->getByName(
"odometry_poses_cloud");
1592 obj->setVisibility(!obj->isVisible());
1594 obj = scene->getByName(
"robot_odometry_poses");
1595 obj->setVisibility(!obj->isVisible());
1606 template<
class GRAPH_T>
1618 CRenderizablePtr obj = scene->getByName(
"GT_cloud");
1619 obj->setVisibility(!obj->isVisible());
1621 obj = scene->getByName(
"robot_GT");
1622 obj->setVisibility(!obj->isVisible());
1633 template<
class GRAPH_T>
1637 using namespace std;
1648 num_of_nodes =
m_graph.nodeCount();
1652 stringstream scan_name(
"");
1654 for (
int node_cnt = 0; node_cnt != num_of_nodes; ++node_cnt) {
1657 scan_name <<
"laser_scan_";
1658 scan_name << node_cnt;
1660 CRenderizablePtr obj = scene->getByName(scan_name.str());
1663 obj->setVisibility(!obj->isVisible());
1671 template<
class GRAPH_T>
1683 CRenderizablePtr obj = scene->getByName(
"estimated_traj_setoflines");
1684 obj->setVisibility(!obj->isVisible());
1686 obj = scene->getByName(
"robot_estimated_traj");
1687 obj->setVisibility(!obj->isVisible());
1698 template<
class GRAPH_T>
1700 std::string viz_flag,
int sleep_time ) {
1705 <<
"Make sure that the corresponding visualization flag (" 1707 <<
") is set to true in the .ini file.");
1713 template<
class GRAPH_T>
1715 const mrpt::obs::CActionCollectionPtr action,
1716 const mrpt::obs::CSensoryFramePtr observations,
1717 const mrpt::obs::CObservationPtr observation) {
1723 ASSERTMSG_(action.present() || observation.present(),
1724 "Neither action or observation contains valid data.");
1727 if (observation.present()) {
1728 timestamp = observation->timestamp;
1732 timestamp = action->get(0).timestamp;
1737 sens_it != observations->end(); ++sens_it) {
1738 timestamp = (*sens_it)->timestamp;
1749 template<
class GRAPH_T>
1755 template<
class GRAPH_T>
1759 CSetOfObjectsPtr map_obj = CSetOfObjects::Create();
1760 map_obj->setName(
"map");
1762 scene->insert(map_obj);
1767 template<
class GRAPH_T>
1771 mrpt::obs::CObservation2DRangeScanPtr>& nodes_to_laser_scans2D,
1772 bool full_update ) {
1778 using namespace std;
1781 CSetOfObjectsPtr map_obj;
1783 CRenderizablePtr obj = scene->getByName(
"map");
1784 map_obj =
static_cast<CSetOfObjectsPtr
>(obj);
1789 map_update_timer.
Tic();
1792 std::set<mrpt::utils::TNodeID> nodes_set;
1797 m_graph.getAllNodes(nodes_set);
1809 node_it = nodes_set.begin();
1810 node_it != nodes_set.end(); ++node_it) {
1813 stringstream scan_name(
"");
1814 scan_name <<
"laser_scan_";
1815 scan_name << *node_it;
1818 CObservation2DRangeScanPtr scan_content;
1821 nodes_to_laser_scans2D.find(*node_it);
1824 if (search != nodes_to_laser_scans2D.end() &&
1825 !(search->second.null())) {
1826 scan_content = search->second;
1834 CRenderizablePtr obj = map_obj->getByName(scan_name.str());
1835 CSetOfObjectsPtr scan_obj =
static_cast<CSetOfObjectsPtr
>(obj);
1836 if (scan_obj.null()) {
1837 scan_obj = CSetOfObjects::Create();
1844 scan_obj->setName(scan_name.str());
1851 stringstream prev_scan_name(
"");
1852 prev_scan_name <<
"laser_scan_" << *node_it - 1;
1853 CRenderizablePtr prev_obj = map_obj->getByName(prev_scan_name.str());
1855 scan_obj->setVisibility(prev_obj->isVisible());
1859 map_obj->insert(scan_obj);
1862 scan_obj =
static_cast<CSetOfObjectsPtr
>(scan_obj);
1867 scan_obj->setPose(scan_pose);
1879 double elapsed_time = map_update_timer.
Tac();
1884 template<
class GRAPH_T>
1887 mrpt::opengl::CSetOfObjectsPtr& viz_object) {
1893 template<
class GRAPH_T>
1897 const int keep_every_n_entries ) {
1901 size_t scan_size = laser_scan_in.
scan.
size();
1904 std::vector<float> new_scan(scan_size);
1905 std::vector<char> new_validRange(scan_size);
1906 size_t new_scan_size = 0;
1907 for (
size_t i=0; i != scan_size; i++) {
1908 if (i % keep_every_n_entries == 0) {
1909 new_scan[new_scan_size] = laser_scan_in.
scan[i];
1910 new_validRange[new_scan_size] = laser_scan_in.
validRange[i];
1914 laser_scan_out->
loadFromVectors(new_scan_size, &new_scan[0], &new_validRange[0]);
1919 template<
class GRAPH_T>
1929 "Visualization of data was requested but no CDisplayWindow3D pointer was provided");
1932 CPointCloudPtr GT_cloud = CPointCloud::Create();
1933 GT_cloud->setPointSize(1.0);
1934 GT_cloud->enablePointSmooth();
1935 GT_cloud->enableColorFromX(
false);
1936 GT_cloud->enableColorFromY(
false);
1937 GT_cloud->enableColorFromZ(
false);
1939 GT_cloud->setName(
"GT_cloud");
1949 scene->insert(GT_cloud);
1950 scene->insert(robot_model);
1966 template<
class GRAPH_T>
1977 "Visualization of data was requested but no CDisplayWindow3D pointer was given");
1981 CRenderizablePtr obj = scene->getByName(
"GT_cloud");
1982 CPointCloudPtr GT_cloud =
static_cast<CPointCloudPtr
>(obj);
1986 GT_cloud->insertPoint(p.
x(), p.
y(), p.z());
1989 obj = scene->getByName(
"robot_GT");
1990 CSetOfObjectsPtr robot_obj =
static_cast<CSetOfObjectsPtr
>(obj);
1991 robot_obj->setPose(p);
1999 template<
class GRAPH_T>
2008 CPointCloudPtr odometry_poses_cloud = CPointCloud::Create();
2009 odometry_poses_cloud->setPointSize(1.0);
2010 odometry_poses_cloud->enablePointSmooth();
2011 odometry_poses_cloud->enableColorFromX(
false);
2012 odometry_poses_cloud->enableColorFromY(
false);
2013 odometry_poses_cloud->enableColorFromZ(
false);
2015 odometry_poses_cloud->setName(
"odometry_poses_cloud");
2019 "robot_odometry_poses",
2025 scene->insert(odometry_poses_cloud);
2026 scene->insert(robot_model);
2042 template<
class GRAPH_T>
2047 "Visualization of data was requested but no CDisplayWindow3D pointer was given");
2053 CRenderizablePtr obj = scene->getByName(
"odometry_poses_cloud");
2054 CPointCloudPtr odometry_poses_cloud =
static_cast<CPointCloudPtr
>(obj);
2057 odometry_poses_cloud->insertPoint(
2063 obj = scene->getByName(
"robot_odometry_poses");
2064 CSetOfObjectsPtr robot_obj =
static_cast<CSetOfObjectsPtr
>(obj);
2065 robot_obj->setPose(p);
2073 template<
class GRAPH_T>
2081 CSetOfLinesPtr estimated_traj_setoflines = CSetOfLines::Create();
2083 estimated_traj_setoflines->setLineWidth(1.5);
2084 estimated_traj_setoflines->setName(
"estimated_traj_setoflines");
2087 estimated_traj_setoflines->appendLine(
2094 "robot_estimated_traj",
2101 scene->insert(estimated_traj_setoflines);
2103 scene->insert(robot_model);
2121 template<
class GRAPH_T>
2133 CRenderizablePtr obj;
2136 obj = scene->getByName(
"estimated_traj_setoflines");
2137 CSetOfLinesPtr estimated_traj_setoflines =
2138 static_cast<CSetOfLinesPtr
>(obj);
2142 std::set<mrpt::utils::TNodeID> nodes_set;
2146 estimated_traj_setoflines->clear();
2148 estimated_traj_setoflines->appendLine(
2153 nodes_set.insert(
m_graph.nodeCount()-1);
2159 it = nodes_set.begin();
2160 it != nodes_set.end(); ++it) {
2164 estimated_traj_setoflines->appendLineStrip(
2173 obj = scene->getByName(
"robot_estimated_traj");
2174 CSetOfObjectsPtr robot_obj =
static_cast<CSetOfObjectsPtr
>(obj);
2176 robot_obj->setPose(curr_estimated_pos);
2187 template<
class GRAPH_T>
2191 this->setRawlogFile(rawlog_fname);
2192 this->initTRGBDInfoFileParams();
2194 template<
class GRAPH_T>
2197 this->initTRGBDInfoFileParams();
2199 template<
class GRAPH_T>
2203 template<
class GRAPH_T>
2205 const std::string& rawlog_fname) {
2210 std::string name_prefix =
"rawlog_";
2211 std::string name_suffix =
"_info.txt";
2212 info_fname = dir + name_prefix + rawlog_filename + name_suffix;
2215 template<
class GRAPH_T>
2219 fields[
"Overall number of objects"] =
"";
2220 fields[
"Observations format"] =
"";
2223 template<
class GRAPH_T>
2226 using namespace std;
2232 "\nTRGBDInfoFileParams::parseFile: Couldn't open info file\n");
2235 size_t line_cnt = 0;
2241 if (curr_line.size() == 0)
2246 while (info_file.
readLine(curr_line)) {
2248 vector<string> curr_tokens;
2259 it != fields.end(); ++it) {
2261 it->second = value_part;
2271 template<
class GRAPH_T>
2273 const std::string* fname_in )
const {
2283 fname =
"output_graph.graph";
2287 m_graph.saveToTextFile(fname);
2292 template<
class GRAPH_T>
2294 const std::string* fname_in )
const {
2297 "\nsave3DScene was called even though enable_visuals flag is off.\nExiting...\n");
2303 if (!scene.present()) {
2311 CPlanarLaserScanPtr laser_scan;
2314 scene->removeObject(laser_scan);
2324 fname =
"output_scene.3DScene";
2328 scene->saveToFile(fname);
2336 template<
class GRAPH_T>
2344 if (
m_graph.nodeCount() < 4 ) {
2356 double trans_diff = 0;
2357 double rot_diff = 0;
2383 index_it = start_it;
2386 curr_node_pos =
m_graph.nodes[index_it->first];
2389 node_delta_pos = curr_node_pos - prev_node_pos;
2390 gt_delta = curr_gt_pos - prev_gt_pos;
2392 trans_diff = gt_delta.distanceTo(node_delta_pos);
2401 prev_node_pos = curr_node_pos;
2402 prev_gt_pos = curr_gt_pos;
2410 template<
class GRAPH_T>
2416 template<
class GRAPH_T>
2430 template<
class GRAPH_T>
2433 using namespace std;
2454 template<
class GRAPH_T>
2463 for (
size_t i = 0; i !=
x.size(); i++) {
2469 "Deformation Energy (x1000)");
2474 xmax = std::max_element(
x.begin(),
x.end());
2475 ymax = std::max_element(y.begin(), y.end());
2478 (xmax !=
x.end()? *xmax : 1),
2480 (ymax != y.end()? *ymax : 1) ) ;
2486 template<
class GRAPH_T>
2489 using namespace std;
2493 stringstream results_ss;
2494 results_ss <<
"Summary: " << std::endl;
2496 results_ss <<
"\tProcessing time: " <<
2499 results_ss <<
"\tReal-time capable: " <<
2501 "TRUE":
"FALSE") << std::endl;
2503 results_ss <<
"\tNum of Nodes: " <<
m_graph.nodeCount() << std::endl;;
2510 const std::string output_res = this->getLogAsString();
2513 report_str->clear();
2515 *report_str += results_ss.str();
2518 *report_str += config_params;
2521 *report_str += time_res;
2524 *report_str += output_res;
2530 template<
class GRAPH_T>
2532 std::map<std::string, int>* node_stats,
2533 std::map<std::string, int>* edge_stats,
2536 using namespace std;
2539 ASSERTMSG_(node_stats,
"Invalid pointer to node_stats is given");
2540 ASSERTMSG_(edge_stats,
"Invalid pointer to edge_stats is given");
2553 (*edge_stats)[it->first] = it->second;
2568 template<
class GRAPH_T>
2570 const std::string& output_dir_fname) {
2574 using namespace mrpt;
2577 format(
"Output directory \"%s\" doesn't exist",
2578 output_dir_fname.c_str()));
2583 std::string report_str;
2585 const std::string ext =
".log";
2600 fname = output_dir_fname +
"/" +
"node_registrar" + ext;
2602 m_node_reg->getDescriptiveReport(&report_str);
2607 fname = output_dir_fname +
"/" +
"edge_registrar" + ext;
2609 m_edge_reg->getDescriptiveReport(&report_str);
2614 fname = output_dir_fname +
"/" +
"optimizer" + ext;
2622 const std::string desc(
"# File includes the evolution of the SLAM metric. Implemented metric computes the \"deformation energy\" that is needed to transfer the estimated trajectory into the ground-truth trajectory (computed as sum of the difference between estimated trajectory and ground truth consecutive poses See \"A comparison of SLAM algorithms based on a graph of relations - W.Burgard et al.\", for more details on the metric.\n");
2624 fname = output_dir_fname +
"/" +
"SLAM_evaluation_metric" + ext;
2637 template<
class GRAPH_T>
2638 mrpt::opengl::CSetOfObjectsPtr
2640 const std::string& model_name,
2642 const size_t model_size,
2643 const pose_t& init_pose) {
2646 ASSERTMSG_(!model_name.empty(),
"Model name provided is empty.");
2648 mrpt::opengl::CSetOfObjectsPtr model =
2650 model->setName(model_name);
2651 model->setColor_u8(model_color);
2652 model->setScale(model_size);
2653 model->setPose(init_pose);
2659 template<
class GRAPH_T>
2661 std::vector<double>* vec_out)
const {
void updateOdometryVisualization()
Update odometry-only cloud with latest odometry estimation.
#define ASSERT_EQUAL_(__A, __B)
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
void registerKeystroke(const std::string key_str, const std::string key_desc)
Make new keystrokes available in the help message box.
double x() const
Common members of all points & poses classes.
CSetOfObjectsPtr OPENGL_IMPEXP RobotPioneer()
Returns a representation of a Pioneer II mobile base.
std::map< mrpt::utils::TNodeID, size_t > m_nodeID_to_gt_indices
Map from nodeIDs to their corresponding closest GT pose index.
void forceRepaint()
Repaints the window. forceRepaint, repaint and updateWindow are all aliases of the same method...
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 updateMapVisualization(const std::map< mrpt::utils::TNodeID, mrpt::obs::CObservation2DRangeScanPtr > &nodes_to_laser_scans2D, bool full_update=false)
Update the map visualization based on the current graphSLAM state.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
mrpt::obs::CObservation2DRangeScanPtr m_first_laser_scan2D
First recorded laser scan - assigned to the root.
int m_text_index_timestamp
void queryObserverForEvents()
Query the observer instance for any user events.
mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > * m_edge_reg
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
mrpt::system::TTimeStamp m_init_timestamp
First recorded timestamp in the dataset.
void parseFile()
Parse the RGBD information file to gain information about the rawlog file contents.
std::string m_keystroke_estimated_trajectory
mrpt::gui::CDisplayWindow3D * m_win
void save3DScene(const std::string *fname_in=NULL) const
Wrapper method around the COpenGLScene::saveToFile method.
bool m_enable_curr_pos_viewport
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
void updateGTVisualization()
Display the next ground truth position in the visualization window.
Create a GUI window and display plots with MATLAB-like interfaces and commands.
void computeMap() const
Compute the map of the environment based on the recorded measurements.
void toggleMapVisualization()
void computeSlamMetric(mrpt::utils::TNodeID nodeID, size_t gt_index)
Compare the SLAM result (estimated trajectory) with the GT path.
std::string BASE_IMPEXP extractFileDirectory(const std::string &filePath)
Extract the whole path (the directory) of a filename from a complete path plus name plus extension...
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::utils::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
A class for storing images as grayscale or RGB bitmaps.
void updateSlamMetricVisualization()
Update the displayPlots window with the new information with regards to the metric.
double m_offset_y_timestamp
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
T y() const
Return y coordinate of the quaternion.
Interface for implementing node registration classes.
void returnEventsStruct(std::map< std::string, bool > *codes_to_pressed, bool reset_keypresses=true)
Return a map of key code to a boolean indicating whether it was pressed since the previous time the c...
void initOdometryVisualization()
double m_dataset_grab_time
Time it took to record the dataset.
iterator begin()
Instance Iterators.
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 m_use_GT
Flag for specifying if we are going to use ground truth data at all.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define THROW_EXCEPTION(msg)
static mrpt::system::TTimeStamp getTimeStamp(const mrpt::obs::CActionCollectionPtr action, const mrpt::obs::CSensoryFramePtr observations, const mrpt::obs::CObservationPtr observation)
Fill the TTimestamp in a consistent manner.
double m_offset_x_left
Offset from the left side of the canvas.
std::string m_keystroke_GT
void loadFromVectors(size_t nRays, const float *scanRanges, const char *scanValidity)
std::string BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
bool execGraphSlamStep(mrpt::obs::CObservationPtr &observation, size_t &rawlog_entry)
Wrapper method around _execGraphSlamStep.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
mrpt::gui::CDisplayWindowPlots * m_win_plot
DisplayPlots instance for visualizing the evolution of the SLAM metric.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
global_pose_t getCurrentRobotPosEstimation() const
Query CGraphSlamEngine instance for the current estimated robot position.
void initRangeImageViewport()
void hold_off()
Disables keeping all the graphs (this is the default behavior).
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
double pitch() const
Get the PITCH angle (in radians)
double getMeanTime(const std::string &name) const
Return the mean execution time of the given "section", or 0 if it hasn't ever been called "enter" wit...
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, const mrpt::opengl::TOpenGLFont font=mrpt::opengl::MRPT_GLUT_BITMAP_TIMES_ROMAN_24)
Add 2D text messages overlapped to the 3D rendered scene.
Interface for implementing edge registration classes.
double yaw() const
Get the YAW angle (in radians)
bool m_request_to_exit
Indicate whether the user wants to exit the application (e.g.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
double m_offset_y_current_constraint_type
const Scalar * const_iterator
mrpt::opengl::CSetOfObjectsPtr setCurrentPositionModel(const std::string &model_name, const mrpt::utils::TColor &model_color=mrpt::utils::TColor(0, 0, 0), const size_t model_size=1, const pose_t &init_pose=pose_t())
Set the opengl model that indicates the latest position of the trajectory at hand.
const_iterator find(const KEY &key) const
void setTextMessageParams(const std::map< std::string, double > &name_to_offset_y, const std::map< std::string, int > &name_to_text_index)
Add the textMessage parameters to the object All the names in the given std::maps have to be already ...
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...
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentLocalTime()
Returns the current (local) time.
void unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
void initGTVisualization()
mrpt::math::CMatrixDouble33 m_rot_TUM_to_MRPT
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
virtual void monitorNodeRegistration(bool registered=false, std::string class_name="Class")
Assert that the given nodes number matches the registered graph nodes, otherwise throw exception...
void loadParams(const std::string &fname)
Read the configuration variables from the .ini file specified by the user.
std::deque< CObservationPtr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
mrpt::utils::TNodeID m_nodeID_max
Internal counter for querying for the number of nodeIDs.
void assignViewportParameters(double *x, double *y, double *width, double *height)
Assign position and size values for the placement of the next viewport.
std::string m_rawlog_fname
Rawlog file from which to read the measurements.
virtual void getRobotEstimatedTrajectory(typename GRAPH_T::global_poses_t *graph_poses) const
virtual void setGraphPtr(GRAPH_T *graph)
Fetch the graph on which the decider/optimizer will work on.
double Tac()
Stops the stopwatch.
void updateAllVisuals()
Wrapper around the deciders/optimizer updateVisuals methods.
void setEdgesManually(const std::string &name, int num_of_edges)
Set number of a specific edge type manually.
GRAPH_T::constraint_t constraint_t
Type of graph constraints.
bool m_observation_only_dataset
void addEdgeType(const std::string &name)
Explicitly register a new edge type.
static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
void BASE_IMPEXP pause(const std::string &msg=std::string("Press any key to continue...")) MRPT_NO_THROWS
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
virtual bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)=0
Generic method for fetching the incremental action/observation readings from the calling function...
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.
static const std::string report_sep
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
virtual void setObjectPropsFromNodeID(const mrpt::utils::TNodeID nodeID, mrpt::opengl::CSetOfObjectsPtr &viz_object)
Set the properties of the map visual object based on the nodeID that it was produced by...
T r() const
Return r coordinate of the quaternion.
GRAPH_T::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
mrpt::gui::CDisplayWindow3D * win
CDisplayWindow instance.
mrpt::graphslam::CWindowObserver * observer
CWindowObserver instance.
uint64_t TNodeID
The type for node IDs in graphs of different types.
virtual void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
void alignOpticalWithMRPTFrame()
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...
void initTRGBDInfoFileParams()
void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager)
Provide the instance with a CWindowManager.
This CStream derived class allow using a file as a write-only, binary stream.
void execDijkstraNodesEstimation()
Wrapper around the GRAPH_T::dijkstra_nodes_estimate.
void updateIntensityImageViewport()
In RGB-D TUM Datasets update the Intensity image displayed in a seperate viewport.
struct mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams m_info_params
mrpt::obs::CObservation3DRangeScanPtr m_last_laser_scan3D
Last laser scan that the current class instance received.
size_t m_GT_poses_step
Rate at which to read the GT poses.
bool m_is_first_time_node_reg
Track the first node registration occurance.
mrpt::utils::TColor m_GT_color
std::map< std::string, std::string > fields
Format for the parameters in the info file: string literal - related value (kept in a string represen...
This class implements a high-performance stopwatch.
void getAsString(std::string *str_out) const
Fill the provided string with a detailed report of the registered, so far, edges. ...
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
bool m_visualize_estimated_trajectory
This namespace contains representation of robot actions and observations.
const bool m_enable_visuals
Determine if we are to enable visualization support or not.
int getLoopClosureEdges() const
Returns the edges that form loop closures in the current graph.
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
std::vector< pose_t > m_odometry_poses
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
virtual void updateCurrPosViewport()
Update the viewport responsible for displaying the graph-building procedure in the estimated position...
bool BASE_IMPEXP directoryExists(const std::string &fileName)
Test if a given directory exists (it fails if the given path refers to an existing file)...
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
CGraphSlamEngine(const std::string &config_file, const std::string &rawlog_fname="", const std::string &fname_GT="", mrpt::graphslam::CWindowManager *win_manager=NULL, mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > *node_reg=NULL, mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > *edge_reg=NULL, mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > *optimizer=NULL)
Constructor of CGraphSlamEngine class template.
mrpt::graphslam::CWindowObserver * m_win_observer
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
uint64_t TNodeID
The type for node IDs in graphs of different types.
void updateEstimatedTrajectoryVisualization(bool full_update=false)
Update the Esstimated robot trajectory with the latest estimated robot position.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > * m_optimizer
double m_curr_deformation_energy
virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager)
Fetch a CWindowManager pointer.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void plot(const std::vector< T1 > &x, const std::vector< T2 > &y, const std::string &lineFormat=std::string("b-"), const std::string &plotName=std::string("plotXY"))
Adds a new layer with a 2D plot based on two vectors of X and Y points, using a MATLAB-like syntax...
void initEstimatedTrajectoryVisualization()
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
mrpt::opengl::CSetOfObjectsPtr initRobotModelVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Method to help overcome the partial template specialization restriction of C++.
virtual ~CGraphSlamEngine()
Default Destructor.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
double roll() const
Get the ROLL angle (in radians)
static const std::string header_sep
Separator string to be used in debugging messages.
void axis(float x_min, float x_max, float y_min, float y_max, bool aspectRatioFix=false)
Set the view area according to the passed coordinated.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
mrpt::utils::TColor m_estimated_traj_color
void toggleEstimatedTrajectoryVisualization()
std::map< std::string, mrpt::utils::CFileOutputStream * >::iterator fstreams_out_it
Map for iterating over output file streams.
void initIntensityImageViewport()
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
std::vector< double > m_deformation_energy_vec
void toggleOdometryVisualization()
static COccupancyGridMap2DPtr Create()
void saveGraph(const std::string *fname_in=NULL) const
Wrapper method around the GRAPH_T::saveToTextFile method.
std::string m_img_external_storage_dir
mrpt::opengl::CSetOfObjectsPtr initRobotModelVisualization()
void setFromMatrix(const Eigen::MatrixBase< Derived > &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
void initMapVisualization()
bool m_user_decides_about_output_dir
void printParams() const
Print the problem parameters to the console for verification.
mrpt::graphslam::CWindowManager * m_win_manager
double m_offset_y_estimated_traj
mrpt::graphslam::detail::CEdgeCounter m_edge_counter
Instance to keep track of all the edges + visualization related operations.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP extractFileName(const std::string &filePath)
Extract just the name (without extension) of a filename from a complete path plus name plus extension...
mrpt::maps::COccupancyGridMap2DPtr m_gridmap_cached
GRAPH_T::global_pose_t global_pose_t
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
static void readGTFile(const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > *gt_poses, std::vector< mrpt::system::TTimeStamp > *gt_timestamps=NULL)
Parse the ground truth .txt file and fill in the corresponding gt_poses vector.
T x() const
Return x coordinate of the quaternion.
pose_t m_curr_odometry_only_pose
Current robot position based solely on odometry.
GRAPH_T m_graph
The graph object to be built and optimized.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
int m_text_index_estimated_traj
bool m_enable_intensity_viewport
double m_offset_y_odometry
std::string m_GT_file_format
int getTotalNumOfEdges() const
Return the total amount of registered edges.
mrpt::synch::CCriticalSection m_graph_section
Mark graph modification/accessing explicitly for multithreaded implementation.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
void clf()
Remove all plot objects in the display (clear and clf do exactly the same).
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
std::string m_keystroke_pause_exec
void Tic()
Starts the stopwatch.
void setRawlogFile(const std::string &rawlog_fname)
bool m_is_paused
Indicated if program is temporarily paused by the user.
size_t m_GT_poses_index
Counter for reading back the GT_poses.
virtual void updateVisuals()
Update the relevant visual features in CDisplayWindow.
std::map< std::string, int >::const_iterator const_iterator
const double & phi() const
Get the phi angle of the 2D pose (in radians)
mrpt::system::TTimeStamp m_curr_timestamp
Current dataset timestamp.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
void enableMousePanZoom(bool enabled)
Enable/disable the feature of pan/zoom with the mouse (default=enabled)
void decimateLaserScan(mrpt::obs::CObservation2DRangeScan &laser_scan_in, mrpt::obs::CObservation2DRangeScan *laser_scan_out, const int keep_every_n_entries=2)
Cut down on the size of the given laser scan.
void getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric...
std::string m_img_prev_path_base
std::vector< pose_t > m_GT_poses
The namespace for 3D scene representation and rendering.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
A RGB color - floats in the range [0,1].
bool m_visualize_odometry_poses
double leave(const char *func_name)
End of a named section.
A matrix of dynamic size.
#define ASSERT_FILE_EXISTS_(FIL)
void addEdge(const std::string &name, bool is_loop_closure=false, bool is_new=false)
Increment the number of edges for the specified type.
mrpt::utils::TColor m_odometry_color
void setName(const std::string &name)
std::string m_keystroke_odometry
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::utils::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
fstreams_out m_out_streams
keeps track of the out fstreams so that they can be closed (if still open) in the class Dtor...
Classes for creating GUI windows for 2D and 3D visualization.
void toggleGTVisualization()
bool getGraphSlamStats(std::map< std::string, int > *node_stats, std::map< std::string, int > *edge_stats, mrpt::system::TTimeStamp *timestamp=NULL)
Fill the given maps with stats regarding the overall execution of graphslam.
mrpt::maps::COctoMapPtr m_octomap_cached
std::vector< std::string > m_supported_constraint_types
MRPT CNetworkOfPoses constraint classes that are currently supported.
std::string getParamsAsString() const
Wrapper around getParamsAsString.
size_t m_robot_model_size
How big are the robots going to be in the scene.
void setPos(int x, int y) MRPT_OVERRIDE
Changes the position of the window on the screen.
mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > * m_node_reg
static void readGTFileRGBD_TUM(const std::string &fname_GT, std::vector< mrpt::poses::CPose2D > *gt_poses, std::vector< mrpt::system::TTimeStamp > *gt_timestamps=NULL)
Parse the ground truth .txt file and fill in the corresponding m_GT_poses vector. ...
mrpt::utils::TColor m_optimized_map_color
std::string BASE_IMPEXP dateTimeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC time): YEAR/MONTH/DAY,HH:MM:SS.MMM.
An observation of the current (cumulative) odometry for a wheeled robot.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
Last laser scan that the current class instance received.
T pow(const T v0, const T v1)
bool m_visualize_SLAM_metric
T z() const
Return z coordinate of the quaternion.
std::string m_current_constraint_type
Type of constraint currently in use.
void enter(const char *func_name)
Start of a named section.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void initCurrPosViewport()
#define ASSERTMSG_(f, __ERROR_MSG)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
int m_text_index_paused_message
Internal auxiliary classes.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Wrapper method that used for printing error messages in a consistent manner.
mrpt::system::TTimeStamp m_map_acq_time
Timestamp at which the map was computed.
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
bool m_map_is_cached
Indicates if the map is cached.
void initSlamMetricVisualization()
mrpt::utils::TColor m_current_constraint_type_color
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
virtual bool _execGraphSlamStep(mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &observation, size_t &rawlog_entry)
Main class method responsible for parsing each measurement and for executing graphSLAM.
void updateRangeImageViewport()
In RGB-D TUM Datasets update the Range image displayed in a seperate viewport.
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
std::string m_config_fname
std::string m_keystroke_map
int m_text_index_odometry
void generateReportFiles(const std::string &output_dir_fname_in)
Generate and write to a corresponding report for each of the respective self/decider/optimizer classe...
virtual void setCriticalSectionPtr(mrpt::synch::CCriticalSection *graph_section)
Fetch a mrpt::synch::CCriticalSection for locking the GRAPH_T resource.
bool m_enable_range_viewport
void getMap(mrpt::maps::COccupancyGridMap2DPtr map, mrpt::system::TTimeStamp *acquisition_time=NULL) const
nodes_to_scans2D_t m_nodes_to_laser_scans2D
Map of NodeIDs to their corresponding LaserScans.
void assignTextMessageParameters(double *offset_y, int *text_index)
Assign the next available offset_y and text_index for the textMessage under construction.
static COctoMapPtr Create()
std::string BASE_IMPEXP timeToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (UTC): HH:MM:SS.MMMMMM.
void initResultsFile(const std::string &fname)
Automate the creation and initialization of a results file relevant to the application.
void setLoopClosureEdgesManually(int num_loop_closures)
Method for manually setting the number of loop closures registered so far.
void initClass()
General initialization method to call from the Class Constructors.
int m_text_index_current_constraint_type