19 template <
class GRAPH_T>
22 m_search_disk_color(142, 142, 56),
23 m_laser_scans_color(0, 20, 255)
45 template <
class GRAPH_T>
55 bool registered_new_node =
false;
57 if (this->m_last_total_num_nodes < this->m_graph->nodeCount())
59 registered_new_node =
true;
60 this->m_last_total_num_nodes = this->m_graph->nodeCount();
69 std::dynamic_pointer_cast<mrpt::obs::CObservation2DRangeScan>(
71 m_is_using_3DScan =
false;
76 std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
81 m_last_laser_scan3D->load();
84 this->convert3DTo2DRangeScan(
86 &m_fake_laser_scan2D);
88 m_is_using_3DScan =
true;
93 if (registered_new_node)
95 if (m_last_laser_scan2D)
97 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
100 "Added laser scans of nodeID: %u",
101 (
unsigned)(this->m_graph->nodeCount() - 1)));
103 if (m_last_laser_scan3D)
105 m_nodes_to_laser_scans3D[this->m_graph->nodeCount() - 1] =
108 "Added laser scans of nodeID: %u",
109 (
unsigned)(this->m_graph->nodeCount() - 1)));
116 m_last_laser_scan2D =
118 if (registered_new_node && m_last_laser_scan2D)
120 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
126 if (registered_new_node)
129 std::set<mrpt::graphs::TNodeID> nodes_to_check_ICP;
130 this->getNearbyNodesOf(
131 &nodes_to_check_ICP, this->m_graph->nodeCount() - 1,
134 "Found * %lu * nodes close to nodeID %lu",
135 nodes_to_check_ICP.size(), this->m_graph->nodeCount() - 1);
138 this->m_just_inserted_lc =
false;
139 registered_new_node =
false;
141 if (m_is_using_3DScan)
143 checkRegistrationCondition3D(nodes_to_check_ICP);
147 checkRegistrationCondition2D(nodes_to_check_ICP);
155 template <
class GRAPH_T>
157 const std::set<mrpt::graphs::TNodeID>& nodes_set)
160 using namespace mrpt;
166 typename nodes_to_scans2D_t::const_iterator search;
169 search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
170 if (search != this->m_nodes_to_laser_scans2D.end())
172 curr_laser_scan = search->second;
187 search = this->m_nodes_to_laser_scans2D.find(node_it);
188 if (search != this->m_nodes_to_laser_scans2D.end())
190 prev_laser_scan = search->second;
193 pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
194 this->m_graph->nodes[node_it];
196 this->m_time_logger.enter(
"CICPCriteriaERD::getICPEdge");
198 *prev_laser_scan, *curr_laser_scan, &rel_edge,
199 &initial_pose, &icp_info);
200 this->m_time_logger.leave(
"CICPCriteriaERD::getICPEdge");
204 ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
207 "ICP constraint between NON-successive nodes: "
208 << node_it <<
" => " << curr_nodeID << std::endl
210 <<
"\tgoodness = " << icp_info.
goodness);
212 "ICP_goodness_thresh: " <<
params.ICP_goodness_thresh);
214 "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"
220 this->registerNewEdge(node_it, curr_nodeID, rel_edge);
221 m_edge_types_to_nums[
"ICP2D"]++;
223 if (
absDiff(curr_nodeID, node_it) >
224 params.LC_min_nodeid_diff)
226 m_edge_types_to_nums[
"LC"]++;
227 this->m_just_inserted_lc =
true;
236 template <
class GRAPH_T>
238 const std::set<mrpt::graphs::TNodeID>& nodes_set)
247 std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation3DRangeScan::Ptr>::
248 const_iterator search;
250 search = m_nodes_to_laser_scans3D.find(curr_nodeID);
251 if (search != m_nodes_to_laser_scans3D.end())
253 curr_laser_scan = search->second;
268 search = m_nodes_to_laser_scans3D.find(node_it);
269 if (search != m_nodes_to_laser_scans3D.end())
271 prev_laser_scan = search->second;
274 this->m_time_logger.enter(
"CICPCriteriaERD::getICPEdge");
276 *prev_laser_scan, *curr_laser_scan, &rel_edge,
nullptr,
278 this->m_time_logger.leave(
"CICPCriteriaERD::getICPEdge");
283 this->registerNewEdge(node_it, curr_nodeID, rel_edge);
284 m_edge_types_to_nums[
"ICP3D"]++;
286 if (
absDiff(curr_nodeID, node_it) >
287 params.LC_min_nodeid_diff)
289 m_edge_types_to_nums[
"LC"]++;
290 this->m_just_inserted_lc =
true;
300 template <
class GRAPH_T>
305 parent_t::registerNewEdge(from, to, rel_edge);
307 this->m_graph->insertEdge(from, to, rel_edge);
310 template <
class GRAPH_T>
312 std::set<mrpt::graphs::TNodeID>* nodes_set,
321 nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
323 double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
324 this->m_graph->nodes[cur_nodeID]);
327 nodes_set->insert(nodeID);
333 this->m_graph->getAllNodes(*nodes_set);
339 template <
class GRAPH_T>
341 const std::map<std::string, bool>& events_occurred)
344 parent_t::notifyOfWindowEvents(events_occurred);
347 if (events_occurred.find(
params.keystroke_laser_scans)->second)
349 this->toggleLaserScansVisualization();
355 template <
class GRAPH_T>
359 ASSERTDEBMSG_(this->m_win,
"No CDisplayWindow3D* was provided");
360 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* was provided");
369 if (
params.visualize_laser_scans)
372 obj->setVisibility(!obj->isVisible());
376 dumpVisibilityErrorMsg(
"visualize_laser_scans");
379 this->m_win->unlockAccess3DScene();
380 this->m_win->forceRepaint();
385 template <
class GRAPH_T>
387 std::map<std::string, int>* edge_types_to_num)
const
391 *edge_types_to_num = m_edge_types_to_nums;
396 template <
class GRAPH_T>
402 this->m_time_logger.enter(
"CICPCriteriaERD::Visuals");
403 parent_t::initializeVisuals();
406 params.has_read_config,
"Configuration parameters aren't loaded yet");
408 this->m_win_observer->registerKeystroke(
409 params.keystroke_laser_scans,
"Toggle LaserScans Visualization");
412 if (
params.ICP_max_distance > 0)
418 obj->setPose(initial_pose);
419 obj->setName(
"ICP_max_distance");
420 obj->setColor_u8(m_search_disk_color);
422 params.ICP_max_distance,
params.ICP_max_distance - 0.1);
425 this->m_win->unlockAccess3DScene();
426 this->m_win->forceRepaint();
430 if (
params.visualize_laser_scans)
436 laser_scan_viz->enablePoints(
true);
437 laser_scan_viz->enableLine(
true);
438 laser_scan_viz->enableSurface(
true);
439 laser_scan_viz->setSurfaceColor(
440 m_laser_scans_color.R, m_laser_scans_color.G, m_laser_scans_color.B,
441 m_laser_scans_color.A);
443 laser_scan_viz->setName(
"laser_scan_viz");
445 scene->insert(laser_scan_viz);
446 this->m_win->unlockAccess3DScene();
447 this->m_win->forceRepaint();
451 if (this->m_win && this->m_win_manager &&
params.ICP_max_distance > 0)
453 this->m_win_manager->assignTextMessageParameters(
454 &m_offset_y_search_disk, &m_text_index_search_disk);
456 this->m_win_manager->addTextMessage(
457 5, -m_offset_y_search_disk,
"ICP Edges search radius",
459 m_text_index_search_disk);
462 this->m_time_logger.leave(
"CICPCriteriaERD::Visuals");
465 template <
class GRAPH_T>
469 this->m_time_logger.enter(
"CICPCriteriaERD::Visuals");
473 parent_t::updateVisuals();
476 if (this->m_win &&
params.ICP_max_distance > 0)
481 CDisk::Ptr disk_obj = std::dynamic_pointer_cast<CDisk>(obj);
483 disk_obj->setPose(this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
485 this->m_win->unlockAccess3DScene();
486 this->m_win->forceRepaint();
490 if (this->m_win &&
params.visualize_laser_scans &&
491 (m_last_laser_scan2D || m_fake_laser_scan2D))
497 std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
500 if (m_fake_laser_scan2D)
502 laser_scan_viz->setScan(*m_fake_laser_scan2D);
506 laser_scan_viz->setScan(*m_last_laser_scan2D);
510 auto search = this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
511 if (search != this->m_graph->nodes.end())
513 laser_scan_viz->setPose(
514 this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
517 laser_scan_viz->setPose(
CPose3D(
518 laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(), -0.15,
519 DEG2RAD(laser_scan_viz->getPoseYaw()),
520 DEG2RAD(laser_scan_viz->getPosePitch()),
521 DEG2RAD(laser_scan_viz->getPoseRoll())));
524 this->m_win->unlockAccess3DScene();
525 this->m_win->forceRepaint();
528 this->m_time_logger.leave(
"CICPCriteriaERD::Visuals");
532 template <
class GRAPH_T>
534 std::string viz_flag,
int sleep_time )
537 using namespace mrpt;
540 "Cannot toggle visibility of specified object.\n "
541 "Make sure that the corresponding visualization flag ( %s "
542 ") is set to true in the .ini file.\n",
544 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
549 template <
class GRAPH_T>
553 parent_t::loadParams(source_fname);
555 params.loadFromConfigFileName(
556 source_fname,
"EdgeRegistrationDeciderParameters");
561 int min_verbosity_level = source.
read_int(
562 "EdgeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
567 template <
class GRAPH_T>
571 parent_t::printParams();
577 template <
class GRAPH_T>
579 std::string* report_str)
const
584 const std::string report_sep(2,
'\n');
585 const std::string header_sep(80,
'#');
588 stringstream class_props_ss;
589 class_props_ss <<
"ICP Goodness-based Registration Procedure Summary: "
591 class_props_ss << header_sep << std::endl;
594 const std::string time_res = this->m_time_logger.getStatsAsText();
595 const std::string output_res = this->getLogAsString();
599 parent_t::getDescriptiveReport(report_str);
601 *report_str += class_props_ss.str();
602 *report_str += report_sep;
604 *report_str += time_res;
605 *report_str += report_sep;
607 *report_str += output_res;
608 *report_str += report_sep;
616 template <
class GRAPH_T>
618 : decider(d), keystroke_laser_scans(
"l"), has_read_config(false)
622 template <
class GRAPH_T>
625 template <
class GRAPH_T>
627 std::ostream&
out)
const
631 out <<
"------------------[ Goodness-based ICP Edge Registration "
632 "]------------------\n";
634 "ICP goodness threshold = %.2f%% \n",
635 ICP_goodness_thresh * 100);
637 "ICP max radius for edge search = %.2f\n", ICP_max_distance);
639 "Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
641 "Visualize laser scans = %d\n", visualize_laser_scans);
643 "3DScans Image Directory = %s\n",
644 scans_img_external_dir.c_str());
648 template <
class GRAPH_T>
654 LC_min_nodeid_diff = source.
read_int(
655 "GeneralConfiguration",
"LC_min_nodeid_diff", 30,
false);
657 source.
read_double(section,
"ICP_max_distance", 10,
false);
658 ICP_goodness_thresh =
659 source.
read_double(section,
"ICP_goodness_thresh", 0.75,
false);
660 visualize_laser_scans = source.
read_bool(
661 "VisualizationParameters",
"visualize_laser_scans",
true,
false);
663 section,
"scan_images_external_directory",
"./",
false);
665 has_read_config =
true;