22 template <
class GRAPH_T>
24 template <
class GRAPH_T>
27 template <
class GRAPH_T>
29 const std::string& config_file,
const std::string& rawlog_fname,
34 : m_node_reg(node_reg),
36 m_optimizer(optimizer),
37 m_enable_visuals(win_manager != nullptr),
38 m_config_fname(config_file),
39 m_rawlog_fname(rawlog_fname),
42 m_win_manager(win_manager),
43 m_paused_message(
"Program is paused. Press \"p/P\" to resume."),
44 m_text_index_paused_message(345),
45 m_odometry_color(0, 0, 255),
46 m_GT_color(0, 255, 0),
47 m_estimated_traj_color(255, 165, 0),
48 m_optimized_map_color(255, 0, 0),
49 m_current_constraint_type_color(255, 255, 255),
50 m_robot_model_size(1),
51 m_class_name(
"CGraphSlamEngine"),
52 m_is_first_time_node_reg(true)
57 template <
class GRAPH_T>
68 template <
class GRAPH_T>
69 typename GRAPH_T::global_pose_t
72 std::lock_guard<std::mutex> graph_lock(m_graph_section);
73 return m_node_reg->getCurrentRobotPosEstimation();
76 template <
class GRAPH_T>
77 typename GRAPH_T::global_poses_t
80 std::lock_guard<std::mutex> graph_lock(m_graph_section);
84 template <
class GRAPH_T>
86 std::set<mrpt::graphs::TNodeID>* nodes_set)
const
89 m_graph.getAllNodes(*nodes_set);
92 template <
class GRAPH_T>
101 m_time_logger.setName(m_class_name);
102 this->logging_enable_keep_record =
true;
103 this->setLoggerName(m_class_name);
115 m_supported_constraint_types.push_back(
"CPosePDFGaussianInf");
116 m_supported_constraint_types.push_back(
"CPose3DPDFGaussianInf");
119 const string c_str(c.GetRuntimeClass()->className);
123 m_supported_constraint_types.begin(),
124 m_supported_constraint_types.end(),
125 c_str) != m_supported_constraint_types.end());
134 "Given graph class " << c_str
135 <<
" has not been tested consistently yet."
136 <<
"Proceed at your own risk.");
141 m_current_constraint_type = c_str;
142 m_current_constraint_type =
"Constraints: " + m_current_constraint_type;
146 if (m_enable_visuals)
148 m_win = m_win_manager->win;
149 m_win_observer = m_win_manager->observer;
154 m_win_observer =
nullptr;
163 m_observation_only_dataset =
false;
164 m_request_to_exit =
false;
170 m_GT_poses_index = 0;
174 m_node_reg->setGraphPtr(&m_graph);
175 m_edge_reg->setGraphPtr(&m_graph);
176 m_optimizer->setGraphPtr(&m_graph);
180 if (m_enable_visuals)
182 m_node_reg->setWindowManagerPtr(m_win_manager);
183 m_edge_reg->setWindowManagerPtr(m_win_manager);
184 m_optimizer->setWindowManagerPtr(m_win_manager);
185 m_edge_counter.setWindowManagerPtr(m_win_manager);
189 m_node_reg->setCriticalSectionPtr(&m_graph_section);
190 m_edge_reg->setCriticalSectionPtr(&m_graph_section);
191 m_optimizer->setCriticalSectionPtr(&m_graph_section);
195 this->loadParams(m_config_fname);
197 if (!m_enable_visuals)
200 m_visualize_odometry_poses = 0;
203 m_visualize_estimated_trajectory = 0;
204 m_visualize_SLAM_metric = 0;
205 m_enable_curr_pos_viewport = 0;
206 m_enable_range_viewport = 0;
207 m_enable_intensity_viewport = 0;
210 m_use_GT = !m_fname_GT.empty();
216 this->alignOpticalWithMRPTFrame();
221 this->readGTFile(m_fname_GT, &m_GT_poses);
229 "Ground truth file was not provided. Switching the related "
230 "visualization parameters off...");
232 m_visualize_SLAM_metric = 0;
236 if (m_enable_visuals)
238 m_win_manager->assignTextMessageParameters(
239 &m_offset_y_timestamp, &m_text_index_timestamp);
244 this->initMapVisualization();
249 if (m_enable_visuals)
252 if (m_visualize_odometry_poses)
254 this->initOdometryVisualization();
259 this->initGTVisualization();
262 this->initEstimatedTrajectoryVisualization();
264 if (m_enable_curr_pos_viewport)
266 this->initCurrPosViewport();
276 std::string rawlog_fname_noext =
279 m_img_external_storage_dir =
280 rawlog_dir + rawlog_fname_noext +
"_Images/";
287 if (m_enable_range_viewport)
289 this->initRangeImageViewport();
291 if (m_enable_intensity_viewport)
293 this->initIntensityImageViewport();
297 if (m_enable_visuals)
302 obj->setFrequency(5);
303 obj->enableTickMarks();
304 obj->setAxisLimits(-10, -10, -10, 10, 10, 10);
305 obj->setName(
"axis");
308 m_win->unlockAccess3DScene();
309 m_win->forceRepaint();
313 if (m_enable_visuals)
316 m_keystroke_pause_exec =
"p";
317 m_keystroke_odometry =
"o";
318 m_keystroke_GT =
"g";
319 m_keystroke_estimated_trajectory =
"t";
320 m_keystroke_map =
"m";
323 m_win_observer->registerKeystroke(
324 m_keystroke_pause_exec,
"Pause/Resume program execution");
325 m_win_observer->registerKeystroke(
326 m_keystroke_odometry,
"Toggle Odometry visualization");
327 m_win_observer->registerKeystroke(
328 m_keystroke_GT,
"Toggle Ground-Truth visualization");
329 m_win_observer->registerKeystroke(
330 m_keystroke_estimated_trajectory,
331 "Toggle Estimated trajectory visualization");
332 m_win_observer->registerKeystroke(
333 m_keystroke_map,
"Toggle Map visualization");
337 vector<string> vec_edge_types;
338 vec_edge_types.emplace_back(
"Odometry");
339 vec_edge_types.emplace_back(
"ICP2D");
340 vec_edge_types.emplace_back(
"ICP3D");
342 for (
auto cit = vec_edge_types.begin(); cit != vec_edge_types.end(); ++cit)
344 m_edge_counter.addEdgeType(*cit);
348 if (m_enable_visuals)
351 double offset_y_total_edges, offset_y_loop_closures;
352 int text_index_total_edges, text_index_loop_closures;
354 m_win_manager->assignTextMessageParameters(
355 &offset_y_total_edges, &text_index_total_edges);
358 map<string, double> name_to_offset_y;
359 map<string, int> name_to_text_index;
360 for (
auto it = vec_edge_types.begin(); it != vec_edge_types.end(); ++it)
362 m_win_manager->assignTextMessageParameters(
363 &name_to_offset_y[*it], &name_to_text_index[*it]);
366 m_win_manager->assignTextMessageParameters(
367 &offset_y_loop_closures, &text_index_loop_closures);
370 m_edge_counter.setTextMessageParams(
371 name_to_offset_y, name_to_text_index, offset_y_total_edges,
372 text_index_total_edges, offset_y_loop_closures,
373 text_index_loop_closures);
377 if (m_enable_visuals)
379 m_win_manager->assignTextMessageParameters(
380 &m_offset_y_current_constraint_type,
381 &m_text_index_current_constraint_type);
382 m_win_manager->addTextMessage(
383 m_offset_x_left, -m_offset_y_current_constraint_type,
384 m_current_constraint_type,
386 m_text_index_current_constraint_type);
390 if (m_enable_visuals)
392 std::lock_guard<std::mutex> graph_lock(m_graph_section);
393 m_time_logger.enter(
"Visuals");
394 m_node_reg->initializeVisuals();
395 m_edge_reg->initializeVisuals();
396 m_optimizer->initializeVisuals();
397 m_time_logger.leave(
"Visuals");
416 gridmap->insertionOptions.maxOccupancyUpdateCertainty = 0.8f;
417 gridmap->insertionOptions.maxDistanceInsertion = 5;
418 gridmap->insertionOptions.wideningBeamsWithDistance =
true;
419 gridmap->insertionOptions.decimation = 2;
421 m_gridmap_cached = gridmap;
422 m_map_is_cached =
false;
431 octomap->insertionOptions.setOccupancyThres(0.5);
432 octomap->insertionOptions.setProbHit(0.7);
433 octomap->insertionOptions.setProbMiss(0.4);
434 octomap->insertionOptions.setClampingThresMin(0.1192);
435 octomap->insertionOptions.setClampingThresMax(0.971);
438 m_map_is_cached =
false;
445 m_info_params.setRawlogFile(m_rawlog_fname);
446 m_info_params.parseFile();
448 int num_of_objects = std::atoi(
449 m_info_params.fields[
"Overall number of objects"].c_str());
450 m_GT_poses_step = m_GT_poses.size() / num_of_objects;
453 "Overall number of objects in rawlog: " << num_of_objects);
455 "Setting the Ground truth read step to: " << m_GT_poses_step);
457 catch (
const std::exception& e)
463 m_curr_deformation_energy = 0;
464 if (m_visualize_SLAM_metric)
466 this->initSlamMetricVisualization();
470 if (m_enable_visuals)
472 this->m_win->addTextMessage(0.5, 0.3,
"", m_text_index_paused_message);
478 template <
class GRAPH_T>
487 return this->_execGraphSlamStep(
488 action, observations, observation, rawlog_entry);
491 template <
class GRAPH_T>
500 using namespace mrpt;
507 m_time_logger.enter(
"proc_time");
510 m_has_read_config,
"\nConfig file is not read yet.\nExiting... \n");
516 m_init_timestamp = getTimeStamp(action, observations, observation);
522 m_observation_only_dataset =
true;
528 m_observation_only_dataset =
false;
531 action->getFirstMovementEstimationMean(increment_pose_3d);
532 pose_t increment_pose(increment_pose_3d);
533 m_curr_odometry_only_pose += increment_pose;
541 bool registered_new_node;
543 std::lock_guard<std::mutex> graph_lock(m_graph_section);
544 m_time_logger.enter(
"node_registrar");
545 registered_new_node =
546 m_node_reg->updateState(action, observations, observation);
547 m_time_logger.leave(
"node_registrar");
552 getObservation<CObservation2DRangeScan>(observations, observation);
555 m_last_laser_scan2D = scan;
557 if (!m_first_laser_scan2D)
559 m_first_laser_scan2D = m_last_laser_scan2D;
564 if (registered_new_node)
568 if (m_is_first_time_node_reg)
570 std::lock_guard<std::mutex> graph_lock(m_graph_section);
572 if (m_graph.nodeCount() != 2)
575 "Expected [2] new registered nodes"
576 <<
" but got [" << m_graph.nodeCount() <<
"]");
580 m_nodes_to_laser_scans2D.insert(
581 make_pair(m_nodeID_max, m_first_laser_scan2D));
583 m_is_first_time_node_reg =
false;
590 m_edge_counter.addEdge(
"Odometry");
592 this->monitorNodeRegistration(
593 registered_new_node,
"NodeRegistrationDecider");
599 std::lock_guard<std::mutex> graph_lock(m_graph_section);
601 m_time_logger.enter(
"edge_registrar");
602 m_edge_reg->updateState(action, observations, observation);
603 m_time_logger.leave(
"edge_registrar");
605 this->monitorNodeRegistration(
606 registered_new_node,
"EdgeRegistrationDecider");
609 std::lock_guard<std::mutex> graph_lock(m_graph_section);
611 m_time_logger.enter(
"optimizer");
612 m_optimizer->updateState(action, observations, observation);
613 m_time_logger.leave(
"optimizer");
615 this->monitorNodeRegistration(registered_new_node,
"GraphSlamOptimizer");
618 m_curr_timestamp = getTimeStamp(action, observations, observation);
629 std::dynamic_pointer_cast<CObservationOdometry>(observation);
631 m_curr_odometry_only_pose =
pose_t(obs_odometry->odometry);
632 m_odometry_poses.push_back(m_curr_odometry_only_pose);
636 m_last_laser_scan3D =
637 std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
649 action->getFirstMovementEstimationMean(increment_pose_3d);
650 pose_t increment_pose(increment_pose_3d);
651 m_curr_odometry_only_pose += increment_pose;
652 m_odometry_poses.push_back(m_curr_odometry_only_pose);
655 if (registered_new_node)
657 this->execDijkstraNodesEstimation();
660 m_nodes_to_laser_scans2D[m_nodeID_max] = m_last_laser_scan2D;
662 if (m_enable_visuals && m_visualize_map)
664 std::lock_guard<std::mutex> graph_lock(m_graph_section);
666 bool full_update =
true;
667 this->updateMapVisualization(m_nodes_to_laser_scans2D, full_update);
671 if (m_enable_visuals)
673 this->updateAllVisuals();
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 for (
auto it = edge_types_to_nums.begin();
682 it != edge_types_to_nums.end(); ++it)
687 m_edge_counter.setLoopClosureEdgesManually(it->second);
691 m_edge_counter.setEdgesManually(it->first, it->second);
698 if (m_enable_curr_pos_viewport)
700 updateCurrPosViewport();
703 if (m_enable_visuals)
705 bool full_update =
true;
706 m_time_logger.enter(
"Visuals");
707 this->updateEstimatedTrajectoryVisualization(full_update);
708 m_time_logger.leave(
"Visuals");
715 m_time_logger.enter(
"SLAM_metric");
716 this->computeSlamMetric(m_nodeID_max, m_GT_poses_index);
717 m_time_logger.leave(
"SLAM_metric");
718 if (m_visualize_SLAM_metric)
720 m_time_logger.enter(
"Visuals");
721 this->updateSlamMetricVisualization();
722 m_time_logger.leave(
"Visuals");
727 m_map_is_cached =
false;
734 m_time_logger.enter(
"Visuals");
738 if (m_enable_visuals)
742 m_win_manager->addTextMessage(
743 m_offset_x_left, -m_offset_y_timestamp,
745 "Simulated time: %s",
748 m_text_index_timestamp);
752 m_win_manager->addTextMessage(
753 m_offset_x_left, -m_offset_y_timestamp,
758 m_text_index_timestamp);
763 if (m_visualize_odometry_poses && m_odometry_poses.size())
765 this->updateOdometryVisualization();
774 if (m_enable_visuals)
776 this->updateGTVisualization();
779 m_GT_poses_index += m_GT_poses_step;
783 if (m_observation_only_dataset)
785 if (rawlog_entry % 2 == 0)
787 if (m_enable_visuals)
789 this->updateGTVisualization();
792 m_GT_poses_index += m_GT_poses_step;
799 if (m_enable_visuals)
801 this->updateGTVisualization();
804 m_GT_poses_index += m_GT_poses_step;
812 if (m_enable_range_viewport && m_last_laser_scan3D)
814 this->updateRangeImageViewport();
817 if (m_enable_intensity_viewport && m_last_laser_scan3D)
819 this->updateIntensityImageViewport();
824 if (m_enable_visuals)
826 this->queryObserverForEvents();
828 m_time_logger.leave(
"Visuals");
830 m_dataset_grab_time =
832 m_time_logger.leave(
"proc_time");
834 return !m_request_to_exit;
838 template <
class GRAPH_T>
842 std::lock_guard<std::mutex> graph_lock(m_graph_section);
843 m_time_logger.enter(
"dijkstra_nodes_estimation");
844 m_graph.dijkstra_nodes_estimate();
845 m_time_logger.leave(
"dijkstra_nodes_estimation");
849 template <
class GRAPH_T>
851 bool registered , std::string class_name )
854 using namespace mrpt;
856 std::lock_guard<std::mutex> graph_lock(m_graph_section);
857 size_t listed_nodeCount =
863 listed_nodeCount == m_graph.nodeCount(),
865 "listed_nodeCount [%lu] != nodeCount() [%lu]",
866 static_cast<unsigned long>(listed_nodeCount),
867 static_cast<unsigned long>(m_graph.nodeCount())));
871 if (listed_nodeCount != m_graph.nodeCount())
874 class_name <<
" illegally added new nodes to the graph "
875 <<
", wanted to see [" << listed_nodeCount
876 <<
"] but saw [" << m_graph.nodeCount() <<
"]");
878 format(
"Illegal node registration by %s.", class_name.c_str()));
884 template <
class GRAPH_T>
897 if (!m_map_is_cached)
901 map->copyMapContentFrom(*m_gridmap_cached);
904 if (acquisition_time)
906 *acquisition_time = m_map_acq_time;
911 template <
class GRAPH_T>
919 if (!m_map_is_cached)
928 if (acquisition_time)
930 *acquisition_time = m_map_acq_time;
936 template <
class GRAPH_T>
941 using namespace mrpt;
945 std::lock_guard<std::mutex> graph_lock(m_graph_section);
947 if (!constraint_t::is_3D())
955 for (
auto it = m_nodes_to_laser_scans2D.begin();
956 it != m_nodes_to_laser_scans2D.end(); ++it)
965 "LaserScan of nodeID: %lu is not present.",
966 static_cast<unsigned long>(curr_node)));
969 CPose3D scan_pose = getLSPoseForGridMapVisualization(curr_node);
972 gridmap->insertObservation(*curr_laser_scan, &scan_pose);
975 m_map_is_cached =
true;
981 THROW_EXCEPTION(
"Not Implemented Yet. Method is to compute a COctoMap");
988 template <
class GRAPH_T>
995 mrpt::format(
"\nConfiguration file not found: \n%s\n", fname.c_str()));
1001 m_user_decides_about_output_dir = cfg_file.
read_bool(
1002 "GeneralConfiguration",
"user_decides_about_output_dir",
false,
false);
1004 "GeneralConfiguration",
"ground_truth_file_format",
"NavSimul",
false);
1007 int min_verbosity_level =
1008 cfg_file.
read_int(
"GeneralConfiguration",
"class_verbosity", 1,
false);
1015 "VisualizationParameters",
"visualize_map",
true,
false);
1018 m_visualize_odometry_poses = cfg_file.
read_bool(
1019 "VisualizationParameters",
"visualize_odometry_poses",
true,
false);
1020 m_visualize_estimated_trajectory = cfg_file.
read_bool(
1021 "VisualizationParameters",
"visualize_estimated_trajectory",
true,
1026 "VisualizationParameters",
"visualize_ground_truth",
true,
false);
1028 m_visualize_SLAM_metric = cfg_file.
read_bool(
1029 "VisualizationParameters",
"visualize_SLAM_metric",
true,
false);
1032 m_enable_curr_pos_viewport = cfg_file.
read_bool(
1033 "VisualizationParameters",
"enable_curr_pos_viewport",
true,
false);
1034 m_enable_range_viewport = cfg_file.
read_bool(
1035 "VisualizationParameters",
"enable_range_viewport",
false,
false);
1036 m_enable_intensity_viewport = cfg_file.
read_bool(
1037 "VisualizationParameters",
"enable_intensity_viewport",
false,
false);
1039 m_node_reg->loadParams(fname);
1040 m_edge_reg->loadParams(fname);
1041 m_optimizer->loadParams(fname);
1043 m_has_read_config =
true;
1046 template <
class GRAPH_T>
1052 this->getParamsAsString(&str);
1057 template <
class GRAPH_T>
1062 using namespace std;
1064 stringstream ss_out;
1067 <<
"\n------------[ Graphslam_engine Problem Parameters ]------------"
1069 ss_out <<
"Config filename = " << m_config_fname
1072 ss_out <<
"Ground Truth File format = " << m_GT_file_format
1074 ss_out <<
"Ground Truth filename = " << m_fname_GT << std::endl;
1076 ss_out <<
"Visualize odometry = "
1077 << (m_visualize_odometry_poses ?
"TRUE" :
"FALSE") << std::endl;
1078 ss_out <<
"Visualize estimated trajectory = "
1079 << (m_visualize_estimated_trajectory ?
"TRUE" :
"FALSE")
1081 ss_out <<
"Visualize map = "
1082 << (m_visualize_map ?
"TRUE" :
"FALSE") << std::endl;
1083 ss_out <<
"Visualize Ground Truth = "
1084 << (m_visualize_GT ?
"TRUE" :
"FALSE") << std::endl;
1086 ss_out <<
"Visualize SLAM metric plot = "
1087 << (m_visualize_SLAM_metric ?
"TRUE" :
"FALSE") << std::endl;
1089 ss_out <<
"Enable curr. position viewport = "
1090 << (m_enable_curr_pos_viewport ?
"TRUE" :
"FALSE") << endl;
1091 ss_out <<
"Enable range img viewport = "
1092 << (m_enable_range_viewport ?
"TRUE" :
"FALSE") << endl;
1093 ss_out <<
"Enable intensity img viewport = "
1094 << (m_enable_intensity_viewport ?
"TRUE" :
"FALSE") << endl;
1096 ss_out <<
"-----------------------------------------------------------"
1098 ss_out << std::endl;
1101 *params_out = ss_out.str();
1106 template <
class GRAPH_T>
1110 std::cout << getParamsAsString();
1112 m_node_reg->printParams();
1113 m_edge_reg->printParams();
1114 m_optimizer->printParams();
1119 template <
class GRAPH_T>
1123 using namespace std;
1130 std::string time_spec =
"UTC Time";
1134 m_out_streams[fname].open(fname);
1136 m_out_streams[fname].fileOpenCorrectly(),
1137 mrpt::format(
"\nError while trying to open %s\n", fname.c_str()));
1139 const std::string sep(80,
'#');
1141 m_out_streams[fname].printf(
"# Mobile Robot Programming Toolkit (MRPT)\n");
1142 m_out_streams[fname].printf(
"# http::/www.mrpt.org\n");
1143 m_out_streams[fname].printf(
"# GraphSlamEngine Application\n");
1144 m_out_streams[fname].printf(
1145 "# Automatically generated file - %s: %s\n", time_spec.c_str(),
1146 cur_date_str.c_str());
1147 m_out_streams[fname].printf(
"%s\n\n", sep.c_str());
1152 template <
class GRAPH_T>
1162 viewp_range = scene->createViewport(
"viewp_range");
1164 m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1165 viewp_range->setViewportPosition(x, y, h, w);
1167 m_win->unlockAccess3DScene();
1168 m_win->forceRepaint();
1173 template <
class GRAPH_T>
1177 std::lock_guard<std::mutex> graph_lock(m_graph_section);
1178 m_time_logger.enter(
"Visuals");
1180 m_node_reg->updateVisuals();
1181 m_edge_reg->updateVisuals();
1182 m_optimizer->updateVisuals();
1184 m_time_logger.leave(
"Visuals");
1188 template <
class GRAPH_T>
1196 if (m_last_laser_scan3D->hasRangeImage)
1204 m_last_laser_scan3D->load();
1206 m_last_laser_scan3D->rangeImage,
false );
1210 viewp_range->setImageView(img);
1211 m_win->unlockAccess3DScene();
1212 m_win->forceRepaint();
1218 template <
class GRAPH_T>
1228 viewp_intensity = scene->createViewport(
"viewp_intensity");
1230 m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1231 viewp_intensity->setViewportPosition(x, y, w, h);
1233 m_win->unlockAccess3DScene();
1234 m_win->forceRepaint();
1238 template <
class GRAPH_T>
1245 if (m_last_laser_scan3D->hasIntensityImage)
1250 m_last_laser_scan3D->load();
1251 img = m_last_laser_scan3D->intensityImage;
1255 scene->getViewport(
"viewp_intensity");
1256 viewp_intensity->setImageView(img);
1257 m_win->unlockAccess3DScene();
1258 m_win->forceRepaint();
1264 template <
class GRAPH_T>
1269 return this->initRobotModelVisualizationInternal(p);
1272 template <
class GRAPH_T>
1281 template <
class GRAPH_T>
1289 template <
class GRAPH_T>
1298 scene->createViewport(
"curr_robot_pose_viewport");
1300 viewp->setCloneView(
"main");
1302 m_win_manager->assignViewportParameters(&x, &y, &w, &h);
1303 viewp->setViewportPosition(x, y, h, w);
1304 viewp->setTransparent(
false);
1305 viewp->getCamera().setAzimuthDegrees(90);
1306 viewp->getCamera().setElevationDegrees(90);
1307 viewp->setCustomBackgroundColor(
1309 viewp->getCamera().setZoomDistance(30);
1310 viewp->getCamera().setOrthogonal();
1312 m_win->unlockAccess3DScene();
1313 m_win->forceRepaint();
1318 template <
class GRAPH_T>
1328 global_pose_t curr_robot_pose = this->getCurrentRobotPosEstimation();
1332 viewp->getCamera().setPointingAt(
CPose3D(curr_robot_pose));
1334 m_win->unlockAccess3DScene();
1335 m_win->forceRepaint();
1341 template <
class GRAPH_T>
1343 const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
1344 std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1347 using namespace std;
1354 "\nGround-truth file %s was not found.\n"
1355 "Either specify a valid ground-truth filename or set set the "
1356 "m_visualize_GT flag to false\n",
1361 ASSERTDEBMSG_(gt_poses,
"\nNo valid std::vector<pose_t>* was given\n");
1366 for (
size_t line_num = 0; file_GT.
readLine(curr_line); line_num++)
1368 vector<string> curr_tokens;
1373 curr_tokens.size() == constraint_t::state_length + 1,
1374 "\nGround Truth File doesn't seem to contain data as generated by "
1376 "GridMapNavSimul application.\n Either specify the GT file format "
1378 "visualize_ground_truth to false in the .ini file\n");
1385 gt_timestamps->push_back(timestamp);
1390 atof(curr_tokens[1].c_str()), atof(curr_tokens[2].c_str()),
1391 atof(curr_tokens[3].c_str()));
1392 gt_poses->push_back(curr_pose);
1399 template <
class GRAPH_T>
1401 const std::string& fname_GT, std::vector<mrpt::poses::CPose3D>* gt_poses,
1402 std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1407 template <
class GRAPH_T>
1409 const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
1410 std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
1413 using namespace std;
1421 "\nGround-truth file %s was not found.\n"
1422 "Either specify a valid ground-truth filename or set set the "
1423 "m_visualize_GT flag to false\n",
1429 "\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
1430 ASSERTDEBMSG_(gt_poses,
"No valid std::vector<pose_t>* was given");
1435 for (
size_t i = 0; file_GT.
readLine(curr_line); i++)
1437 if (curr_line.at(0) !=
'#')
1446 vector<string> curr_tokens;
1451 curr_tokens.size() == 8,
1452 "\nGround Truth File doesn't seem to contain data as specified in "
1454 "datasets.\n Either specify correct the GT file format or set "
1455 "visualize_ground_truth to false in the .ini file\n");
1459 quat.
r(atof(curr_tokens[7].c_str()));
1460 quat.
x(atof(curr_tokens[4].c_str()));
1461 quat.
y(atof(curr_tokens[5].c_str()));
1462 quat.
z(atof(curr_tokens[6].c_str()));
1467 curr_coords[0] = atof(curr_tokens[1].c_str());
1468 curr_coords[1] = atof(curr_tokens[2].c_str());
1469 curr_coords[2] = atof(curr_tokens[3].c_str());
1472 pose_t curr_pose(curr_coords[0], curr_coords[1], y);
1475 pose_diff = curr_pose;
1478 for (; file_GT.
readLine(curr_line);)
1482 curr_tokens.size() == 8,
1483 "\nGround Truth File doesn't seem to contain data as specified in "
1485 "datasets.\n Either specify correct the GT file format or set "
1486 "visualize_ground_truth to false in the .ini file\n");
1493 gt_timestamps->push_back(timestamp);
1497 quat.
r(atof(curr_tokens[7].c_str()));
1498 quat.
x(atof(curr_tokens[4].c_str()));
1499 quat.
y(atof(curr_tokens[5].c_str()));
1500 quat.
z(atof(curr_tokens[6].c_str()));
1504 curr_coords[0] = atof(curr_tokens[1].c_str());
1505 curr_coords[1] = atof(curr_tokens[2].c_str());
1506 curr_coords[2] = atof(curr_tokens[3].c_str());
1509 pose_t gt_pose(curr_coords[0], curr_coords[1], y);
1511 gt_pose.x() -= pose_diff.x();
1512 gt_pose.y() -= pose_diff.y();
1513 gt_pose.phi() -= pose_diff.phi();
1515 gt_poses->push_back(gt_pose);
1523 template <
class GRAPH_T>
1527 using namespace std;
1536 double anglez = 0.0_deg;
1537 const double tmpz[] = {
1538 cos(anglez), -sin(anglez), 0, sin(anglez), cos(anglez), 0, 0, 0, 1};
1542 double angley = 0.0_deg;
1544 const double tmpy[] = {cos(angley), 0, sin(angley), 0, 1, 0,
1545 -sin(angley), 0, cos(angley)};
1550 double anglex = 0.0_deg;
1551 const double tmpx[] = {
1552 1, 0, 0, 0, cos(anglex), -sin(anglex), 0, sin(anglex), cos(anglex)};
1555 stringstream ss_out;
1556 ss_out <<
"\nConstructing the rotation matrix for the GroundTruth Data..."
1558 m_rot_TUM_to_MRPT = rotz * roty * rotx;
1560 ss_out <<
"Rotation matrices for optical=>MRPT transformation" << endl;
1561 ss_out <<
"rotz: " << endl << rotz << endl;
1562 ss_out <<
"roty: " << endl << roty << endl;
1563 ss_out <<
"rotx: " << endl << rotx << endl;
1564 ss_out <<
"Full rotation matrix: " << endl << m_rot_TUM_to_MRPT << endl;
1571 template <
class GRAPH_T>
1578 "\nqueryObserverForEvents method was called even though no Observer "
1579 "object was provided\n");
1581 std::map<std::string, bool> events_occurred;
1582 m_win_observer->returnEventsStruct(&events_occurred);
1583 m_request_to_exit = events_occurred.find(
"Ctrl+c")->second;
1586 if (events_occurred[m_keystroke_odometry])
1588 this->toggleOdometryVisualization();
1591 if (events_occurred[m_keystroke_GT])
1593 this->toggleGTVisualization();
1596 if (events_occurred[m_keystroke_map])
1598 this->toggleMapVisualization();
1601 if (events_occurred[m_keystroke_estimated_trajectory])
1603 this->toggleEstimatedTrajectoryVisualization();
1606 if (events_occurred[m_keystroke_pause_exec])
1608 this->togglePause();
1613 m_node_reg->notifyOfWindowEvents(events_occurred);
1614 m_edge_reg->notifyOfWindowEvents(events_occurred);
1615 m_optimizer->notifyOfWindowEvents(events_occurred);
1620 template <
class GRAPH_T>
1630 if (m_visualize_odometry_poses)
1633 obj->setVisibility(!obj->isVisible());
1635 obj = scene->getByName(
"robot_odometry_poses");
1636 obj->setVisibility(!obj->isVisible());
1640 dumpVisibilityErrorMsg(
"visualize_odometry_poses");
1643 m_win->unlockAccess3DScene();
1644 m_win->forceRepaint();
1648 template <
class GRAPH_T>
1661 obj->setVisibility(!obj->isVisible());
1663 obj = scene->getByName(
"robot_GT");
1664 obj->setVisibility(!obj->isVisible());
1668 dumpVisibilityErrorMsg(
"visualize_ground_truth");
1671 m_win->unlockAccess3DScene();
1672 m_win->forceRepaint();
1676 template <
class GRAPH_T>
1681 using namespace std;
1690 std::lock_guard<std::mutex> graph_lock(m_graph_section);
1691 num_of_nodes = m_graph.nodeCount();
1695 stringstream scan_name(
"");
1697 for (
int node_cnt = 0; node_cnt != num_of_nodes; ++node_cnt)
1701 scan_name <<
"laser_scan_";
1702 scan_name << node_cnt;
1709 obj->setVisibility(!obj->isVisible());
1712 m_win->unlockAccess3DScene();
1713 m_win->forceRepaint();
1717 template <
class GRAPH_T>
1727 if (m_visualize_estimated_trajectory)
1730 obj->setVisibility(!obj->isVisible());
1732 obj = scene->getByName(
"robot_estimated_traj");
1733 obj->setVisibility(!obj->isVisible());
1737 dumpVisibilityErrorMsg(
"visualize_estimated_trajectory");
1740 m_win->unlockAccess3DScene();
1741 m_win->forceRepaint();
1745 template <
class GRAPH_T>
1747 std::string viz_flag,
int sleep_time)
1752 "Cannot toggle visibility of specified object.\n"
1753 <<
"Make sure that the corresponding visualization flag (" << viz_flag
1754 <<
") is set to true in the .ini file.");
1755 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
1760 template <
class GRAPH_T>
1772 action || observation,
1773 "Neither action or observation contains valid data.");
1778 timestamp = observation->timestamp;
1783 timestamp = action->get(0)->timestamp;
1788 for (
auto sens_it = observations->begin();
1789 sens_it != observations->end(); ++sens_it)
1791 timestamp = (*sens_it)->timestamp;
1803 template <
class GRAPH_T>
1811 template <
class GRAPH_T>
1817 map_obj->setName(
"map");
1819 scene->insert(map_obj);
1820 this->m_win->unlockAccess3DScene();
1821 this->m_win->forceRepaint();
1824 template <
class GRAPH_T>
1828 nodes_to_laser_scans2D,
1835 using namespace std;
1841 map_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1846 map_update_timer.
Tic();
1849 std::set<mrpt::graphs::TNodeID> nodes_set;
1856 m_graph.getAllNodes(nodes_set);
1863 nodes_set.insert(m_nodeID_max);
1871 stringstream scan_name(
"");
1872 scan_name <<
"laser_scan_";
1873 scan_name << node_it;
1877 auto search = nodes_to_laser_scans2D.find(node_it);
1880 if (search != nodes_to_laser_scans2D.end() && search->second)
1882 scan_content = search->second;
1885 this->decimateLaserScan(
1886 *scan_content, &scan_decimated,
1894 std::dynamic_pointer_cast<CSetOfObjects>(obj);
1897 scan_obj = std::make_shared<CSetOfObjects>();
1904 scan_obj->setName(scan_name.str());
1905 this->setObjectPropsFromNodeID(node_it, scan_obj);
1913 stringstream prev_scan_name(
"");
1914 prev_scan_name <<
"laser_scan_" << node_it - 1;
1916 map_obj->getByName(prev_scan_name.str());
1919 scan_obj->setVisibility(prev_obj->isVisible());
1923 map_obj->insert(scan_obj);
1927 scan_obj = std::dynamic_pointer_cast<CSetOfObjects>(scan_obj);
1932 scan_obj->setPose(scan_pose);
1937 "Laser scans of NodeID " << node_it <<
"are empty/invalid");
1942 m_win->unlockAccess3DScene();
1943 m_win->forceRepaint();
1945 double elapsed_time = map_update_timer.
Tac();
1947 "updateMapVisualization took: " << elapsed_time <<
"s");
1951 template <
class GRAPH_T>
1957 viz_object->setColor_u8(m_optimized_map_color);
1961 template <
class GRAPH_T>
1965 const int keep_every_n_entries )
1972 std::vector<float> new_scan(
1974 std::vector<char> new_validRange(scan_size);
1975 size_t new_scan_size = 0;
1976 for (
size_t i = 0; i != scan_size; i++)
1978 if (i % keep_every_n_entries == 0)
1980 new_scan[new_scan_size] = laser_scan_in.
getScanRange(i);
1981 new_validRange[new_scan_size] =
1987 new_scan_size, &new_scan[0], &new_validRange[0]);
1992 template <
class GRAPH_T>
2003 "Visualization of data was requested but no CDisplayWindow3D pointer "
2008 GT_cloud->setPointSize(1.0);
2009 GT_cloud->enableColorFromX(
false);
2010 GT_cloud->enableColorFromY(
false);
2011 GT_cloud->enableColorFromZ(
false);
2012 GT_cloud->setColor_u8(m_GT_color);
2013 GT_cloud->setName(
"GT_cloud");
2017 "robot_GT", m_GT_color, m_robot_model_size);
2021 scene->insert(GT_cloud);
2022 scene->insert(robot_model);
2023 m_win->unlockAccess3DScene();
2025 m_win_manager->assignTextMessageParameters(
2026 &m_offset_y_GT, &m_text_index_GT);
2027 m_win_manager->addTextMessage(
2028 m_offset_x_left, -m_offset_y_GT,
"Ground truth path",
2030 m_win->forceRepaint();
2035 template <
class GRAPH_T>
2044 if (m_visualize_GT && m_GT_poses_index < m_GT_poses.size())
2048 "Visualization of data was requested but no CDisplayWindow3D "
2049 "pointer was given");
2058 GT_cloud->insertPoint(p.
x(), p.
y(), p.z());
2061 obj = scene->getByName(
"robot_GT");
2063 std::dynamic_pointer_cast<CSetOfObjects>(obj);
2064 robot_obj->setPose(p);
2065 m_win->unlockAccess3DScene();
2066 m_win->forceRepaint();
2072 template <
class GRAPH_T>
2082 odometry_poses_cloud->setPointSize(1.0);
2083 odometry_poses_cloud->enableColorFromX(
false);
2084 odometry_poses_cloud->enableColorFromY(
false);
2085 odometry_poses_cloud->enableColorFromZ(
false);
2086 odometry_poses_cloud->setColor_u8(m_odometry_color);
2087 odometry_poses_cloud->setName(
"odometry_poses_cloud");
2091 "robot_odometry_poses", m_odometry_color, m_robot_model_size);
2095 scene->insert(odometry_poses_cloud);
2096 scene->insert(robot_model);
2097 m_win->unlockAccess3DScene();
2099 m_win_manager->assignTextMessageParameters(
2100 &m_offset_y_odometry, &m_text_index_odometry);
2101 m_win_manager->addTextMessage(
2102 m_offset_x_left, -m_offset_y_odometry,
"Odometry path",
2105 m_win->forceRepaint();
2110 template <
class GRAPH_T>
2117 "Visualization of data was requested but no CDisplayWindow3D pointer "
2126 std::dynamic_pointer_cast<CPointCloud>(obj);
2129 odometry_poses_cloud->insertPoint(p.
x(), p.
y(), p.z());
2132 obj = scene->getByName(
"robot_odometry_poses");
2134 std::dynamic_pointer_cast<CSetOfObjects>(obj);
2135 robot_obj->setPose(p);
2137 m_win->unlockAccess3DScene();
2138 m_win->forceRepaint();
2143 template <
class GRAPH_T>
2152 std::make_shared<CSetOfLines>();
2153 estimated_traj_setoflines->setColor_u8(m_estimated_traj_color);
2154 estimated_traj_setoflines->setLineWidth(1.5);
2155 estimated_traj_setoflines->setName(
"estimated_traj_setoflines");
2158 estimated_traj_setoflines->appendLine(
2165 "robot_estimated_traj", m_estimated_traj_color, m_robot_model_size);
2169 if (m_visualize_estimated_trajectory)
2171 scene->insert(estimated_traj_setoflines);
2173 scene->insert(robot_model);
2174 m_win->unlockAccess3DScene();
2176 if (m_visualize_estimated_trajectory)
2178 m_win_manager->assignTextMessageParameters(
2179 &m_offset_y_estimated_traj, &m_text_index_estimated_traj);
2180 m_win_manager->addTextMessage(
2181 m_offset_x_left, -m_offset_y_estimated_traj,
"Estimated trajectory",
2183 m_text_index_estimated_traj);
2186 m_win->forceRepaint();
2191 template <
class GRAPH_T>
2199 std::lock_guard<std::mutex> graph_lock(m_graph_section);
2205 if (m_visualize_estimated_trajectory)
2208 obj = scene->getByName(
"estimated_traj_setoflines");
2210 std::dynamic_pointer_cast<CSetOfLines>(obj);
2214 std::set<mrpt::graphs::TNodeID> nodes_set;
2218 this->getNodeIDsOfEstimatedTrajectory(&nodes_set);
2219 estimated_traj_setoflines->clear();
2221 estimated_traj_setoflines->appendLine(
2227 nodes_set.insert(m_graph.nodeCount() - 1);
2231 for (
unsigned long it : nodes_set)
2235 estimated_traj_setoflines->appendLineStrip(p.
x(), p.
y(), p.z());
2241 obj = scene->getByName(
"robot_estimated_traj");
2243 std::dynamic_pointer_cast<CSetOfObjects>(obj);
2244 pose_t curr_estimated_pos = m_graph.nodes[m_graph.nodeCount() - 1];
2245 robot_obj->setPose(curr_estimated_pos);
2247 m_win->unlockAccess3DScene();
2248 m_win->forceRepaint();
2252 template <
class GRAPH_T>
2254 const std::string& rawlog_fname)
2256 this->setRawlogFile(rawlog_fname);
2257 this->initTRGBDInfoFileParams();
2259 template <
class GRAPH_T>
2262 this->initTRGBDInfoFileParams();
2265 template <
class GRAPH_T>
2267 const std::string& rawlog_fname)
2272 std::string name_prefix =
"rawlog_";
2273 std::string name_suffix =
"_info.txt";
2274 info_fname =
dir + name_prefix + rawlog_filename + name_suffix;
2277 template <
class GRAPH_T>
2281 fields[
"Overall number of objects"] =
"";
2282 fields[
"Observations format"] =
"";
2285 template <
class GRAPH_T>
2289 using namespace std;
2295 "\nTRGBDInfoFileParams::parseFile: Couldn't open info file\n");
2298 size_t line_cnt = 0;
2305 if (curr_line.size() == 0)
break;
2309 while (info_file.
readLine(curr_line))
2312 vector<string> curr_tokens;
2322 for (
auto it = fields.begin(); it != fields.end(); ++it)
2326 it->second = value_part;
2334 template <
class GRAPH_T>
2347 fname =
"output_graph.graph";
2351 "Saving generated graph in VERTEX/EDGE format: " << fname);
2352 m_graph.saveToTextFile(fname);
2357 template <
class GRAPH_T>
2363 "\nsave3DScene was called even though enable_visuals flag is "
2364 "off.\nExiting...\n");
2372 "Could not fetch 3D Scene. Display window must already be closed.");
2383 "Removing CPlanarLaserScan from generated 3DScene...");
2384 scene->removeObject(laser_scan);
2396 fname =
"output_scene.3DScene";
2400 scene->saveToFile(fname);
2402 m_win->unlockAccess3DScene();
2403 m_win->forceRepaint();
2408 template <
class GRAPH_T>
2417 if (m_graph.nodeCount() < 4)
2423 m_nodeID_to_gt_indices[nodeID] = gt_index;
2430 double trans_diff = 0;
2431 double rot_diff = 0;
2433 size_t indices_size = m_nodeID_to_gt_indices.size();
2436 m_curr_deformation_energy = 0;
2439 auto start_it = m_nodeID_to_gt_indices.begin();
2443 auto prev_it = start_it;
2445 pose_t prev_node_pos = m_graph.nodes[prev_it->first];
2446 pose_t prev_gt_pos = m_GT_poses[prev_it->second];
2451 for (
auto index_it = start_it; index_it != m_nodeID_to_gt_indices.end();
2454 curr_node_pos = m_graph.nodes[index_it->first];
2455 curr_gt_pos = m_GT_poses[index_it->second];
2457 node_delta_pos = curr_node_pos - prev_node_pos;
2458 gt_delta = curr_gt_pos - prev_gt_pos;
2460 trans_diff = gt_delta.distanceTo(node_delta_pos);
2461 rot_diff = this->accumulateAngleDiffs(gt_delta, node_delta_pos);
2463 m_curr_deformation_energy += (pow(trans_diff, 2) + pow(rot_diff, 2));
2464 m_curr_deformation_energy /= indices_size;
2467 m_deformation_energy_vec.push_back(m_curr_deformation_energy);
2469 prev_node_pos = curr_node_pos;
2470 prev_gt_pos = curr_gt_pos;
2474 "Total deformation energy: " << m_curr_deformation_energy);
2479 template <
class GRAPH_T>
2485 template <
class GRAPH_T>
2499 template <
class GRAPH_T>
2509 m_win_plot = std::make_unique<mrpt::gui::CDisplayWindowPlots>(
2510 "Evolution of SLAM metric - Deformation Energy (1:1000)", 400, 300);
2512 m_win_plot->setPos(20, 50);
2515 m_win_plot->hold_off();
2516 m_win_plot->enableMousePanZoom(
true);
2521 template <
class GRAPH_T>
2526 ASSERTDEB_(m_win_plot && m_visualize_SLAM_metric);
2529 std::vector<double> x(m_deformation_energy_vec.size(), 0);
2530 std::vector<double> y(m_deformation_energy_vec.size(), 0);
2531 for (
size_t i = 0; i != x.size(); i++)
2534 y[i] = m_deformation_energy_vec[i] * 1000;
2537 m_win_plot->plot(x, y,
"r-1",
"Deformation Energy (x1000)");
2541 std::vector<double>::const_iterator xmax, ymax;
2542 xmax = std::max_element(x.begin(), x.end());
2543 ymax = std::max_element(y.begin(), y.end());
2546 xmax != x.end() ? -(*xmax / 12) : -1,
2547 (xmax != x.end() ? *xmax : 1),
2549 (ymax != y.end() ? *ymax : 1));
2554 template <
class GRAPH_T>
2556 std::string* report_str)
const
2559 using namespace std;
2562 stringstream results_ss;
2563 results_ss <<
"Summary: " << std::endl;
2564 results_ss << this->header_sep << std::endl;
2565 results_ss <<
"\tProcessing time: "
2566 << m_time_logger.getMeanTime(
"proc_time") << std::endl;
2568 results_ss <<
"\tDataset Grab time: " << m_dataset_grab_time << std::endl;
2569 results_ss <<
"\tReal-time capable: "
2570 << (m_time_logger.getMeanTime(
"proc_time") < m_dataset_grab_time
2574 results_ss << m_edge_counter.getAsString();
2575 results_ss <<
"\tNum of Nodes: " << m_graph.nodeCount() << std::endl;
2579 std::string config_params = this->getParamsAsString();
2582 const std::string time_res = m_time_logger.getStatsAsText();
2583 const std::string output_res = this->getLogAsString();
2586 report_str->clear();
2588 *report_str += results_ss.str();
2589 *report_str += this->report_sep;
2591 *report_str += config_params;
2592 *report_str += this->report_sep;
2594 *report_str += time_res;
2595 *report_str += this->report_sep;
2597 *report_str += output_res;
2598 *report_str += this->report_sep;
2603 template <
class GRAPH_T>
2605 std::map<std::string, int>* node_stats,
2609 using namespace std;
2612 ASSERTDEBMSG_(node_stats,
"Invalid pointer to node_stats is given");
2613 ASSERTDEBMSG_(edge_stats,
"Invalid pointer to edge_stats is given");
2615 if (m_nodeID_max == 0)
2621 (*node_stats)[
"nodes_total"] = m_nodeID_max + 1;
2624 for (
auto it = m_edge_counter.cbegin(); it != m_edge_counter.cend(); ++it)
2626 (*edge_stats)[it->first] = it->second;
2629 (*edge_stats)[
"loop_closures"] = m_edge_counter.getLoopClosureEdges();
2630 (*edge_stats)[
"edges_total"] = m_edge_counter.getTotalNumOfEdges();
2635 *timestamp = m_curr_timestamp;
2642 template <
class GRAPH_T>
2644 const std::string& output_dir_fname)
2648 using namespace mrpt;
2653 "Output directory \"%s\" doesn't exist", output_dir_fname.c_str()));
2656 std::lock_guard<std::mutex> graph_lock(m_graph_section);
2658 std::string report_str;
2660 const std::string ext =
".log";
2664 fname = output_dir_fname +
"/" + m_class_name + ext;
2667 this->initResultsFile(fname);
2668 this->getDescriptiveReport(&report_str);
2669 m_out_streams[fname].printf(
"%s", report_str.c_str());
2672 const std::string time_res = m_time_logger.getStatsAsText();
2673 fname = output_dir_fname +
"/" +
"timings" + ext;
2674 this->initResultsFile(fname);
2675 m_out_streams[fname].printf(
"%s", time_res.c_str());
2679 fname = output_dir_fname +
"/" +
"node_registrar" + ext;
2680 this->initResultsFile(fname);
2681 m_node_reg->getDescriptiveReport(&report_str);
2682 m_out_streams[fname].printf(
"%s", report_str.c_str());
2686 fname = output_dir_fname +
"/" +
"edge_registrar" + ext;
2687 this->initResultsFile(fname);
2688 m_edge_reg->getDescriptiveReport(&report_str);
2689 m_out_streams[fname].printf(
"%s", report_str.c_str());
2693 fname = output_dir_fname +
"/" +
"optimizer" + ext;
2694 this->initResultsFile(fname);
2695 m_optimizer->getDescriptiveReport(&report_str);
2696 m_out_streams[fname].printf(
"%s", report_str.c_str());
2702 const std::string desc(
2703 "# File includes the evolution of the SLAM metric. Implemented "
2704 "metric computes the \"deformation energy\" that is needed to "
2705 "transfer the estimated trajectory into the ground-truth "
2706 "trajectory (computed as sum of the difference between estimated "
2707 "trajectory and ground truth consecutive poses See \"A comparison "
2708 "of SLAM algorithms based on a graph of relations - W.Burgard et "
2709 "al.\", for more details on the metric.\n");
2711 fname = output_dir_fname +
"/" +
"SLAM_evaluation_metric" + ext;
2712 this->initResultsFile(fname);
2714 m_out_streams[fname].printf(
"%s\n", desc.c_str());
2715 for (
auto vec_it = m_deformation_energy_vec.begin();
2716 vec_it != m_deformation_energy_vec.end(); ++vec_it)
2718 m_out_streams[fname].printf(
"%f\n", *vec_it);
2724 template <
class GRAPH_T>
2728 const size_t model_size,
const pose_t& init_pose)
2731 this->initRobotModelVisualization();
2732 model->setName(model_name);
2733 model->setColor_u8(model_color);
2734 model->setScale(model_size);
2735 model->setPose(init_pose);
2740 template <
class GRAPH_T>
2742 std::vector<double>* vec_out)
const
2744 *vec_out = m_deformation_energy_vec;