10 #ifndef CICPCRITERIAERD_IMPL_H
11 #define CICPCRITERIAERD_IMPL_H
23 template <
class GRAPH_T>
26 m_search_disk_color(142, 142, 56),
27 m_laser_scans_color(0, 20, 255),
28 m_is_using_3DScan(false)
45 template <
class GRAPH_T>
53 template <
class GRAPH_T>
64 bool registered_new_node =
false;
66 if (this->m_last_total_num_nodes < this->m_graph->nodeCount())
68 registered_new_node =
true;
69 this->m_last_total_num_nodes = this->m_graph->nodeCount();
78 std::dynamic_pointer_cast<mrpt::obs::CObservation2DRangeScan>(
80 m_is_using_3DScan =
false;
85 std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
90 m_last_laser_scan3D->load();
93 this->convert3DTo2DRangeScan(
95 &m_fake_laser_scan2D);
97 m_is_using_3DScan =
true;
102 if (registered_new_node)
104 if (m_last_laser_scan2D)
106 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
110 "Added laser scans of nodeID: %u",
111 (
unsigned)(this->m_graph->nodeCount() - 1)));
113 if (m_last_laser_scan3D)
115 m_nodes_to_laser_scans3D[this->m_graph->nodeCount() - 1] =
119 "Added laser scans of nodeID: %u",
120 (
unsigned)(this->m_graph->nodeCount() - 1)));
127 m_last_laser_scan2D =
129 if (registered_new_node && m_last_laser_scan2D)
131 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
137 if (registered_new_node)
140 std::set<mrpt::graphs::TNodeID> nodes_to_check_ICP;
141 this->getNearbyNodesOf(
142 &nodes_to_check_ICP, this->m_graph->nodeCount() - 1,
145 "Found * %lu * nodes close to nodeID %lu",
146 nodes_to_check_ICP.size(), this->m_graph->nodeCount() - 1);
149 this->m_just_inserted_lc =
false;
150 registered_new_node =
false;
152 if (m_is_using_3DScan)
154 checkRegistrationCondition3D(nodes_to_check_ICP);
158 checkRegistrationCondition2D(nodes_to_check_ICP);
166 template <
class GRAPH_T>
168 const std::set<mrpt::graphs::TNodeID>& nodes_set)
171 using namespace mrpt;
180 search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
181 if (search != this->m_nodes_to_laser_scans2D.end())
183 curr_laser_scan = search->second;
192 node_it != nodes_set.end(); ++node_it)
200 search = this->m_nodes_to_laser_scans2D.find(*node_it);
201 if (search != this->m_nodes_to_laser_scans2D.end())
203 prev_laser_scan = search->second;
206 pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
207 this->m_graph->nodes[*node_it];
209 this->m_time_logger.enter(
"CICPCriteriaERD::getICPEdge");
211 *prev_laser_scan, *curr_laser_scan, &rel_edge,
212 &initial_pose, &icp_info);
213 this->m_time_logger.leave(
"CICPCriteriaERD::getICPEdge");
217 ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
220 "ICP constraint between NON-successive nodes: "
221 << *node_it <<
" => " << curr_nodeID << std::endl
223 <<
"\tgoodness = " << icp_info.
goodness);
225 "ICP_goodness_thresh: " <<
params.ICP_goodness_thresh);
227 "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"
233 this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
234 m_edge_types_to_nums[
"ICP2D"]++;
236 if (
absDiff(curr_nodeID, *node_it) >
237 params.LC_min_nodeid_diff)
239 m_edge_types_to_nums[
"LC"]++;
240 this->m_just_inserted_lc =
true;
249 template <
class GRAPH_T>
251 const std::set<mrpt::graphs::TNodeID>& nodes_set)
263 search = m_nodes_to_laser_scans3D.find(curr_nodeID);
264 if (search != m_nodes_to_laser_scans3D.end())
266 curr_laser_scan = search->second;
275 node_it != nodes_set.end(); ++node_it)
283 search = m_nodes_to_laser_scans3D.find(*node_it);
284 if (search != m_nodes_to_laser_scans3D.end())
286 prev_laser_scan = search->second;
289 this->m_time_logger.enter(
"CICPCriteriaERD::getICPEdge");
291 *prev_laser_scan, *curr_laser_scan, &rel_edge,
nullptr,
293 this->m_time_logger.leave(
"CICPCriteriaERD::getICPEdge");
298 this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
299 m_edge_types_to_nums[
"ICP3D"]++;
301 if (
absDiff(curr_nodeID, *node_it) >
302 params.LC_min_nodeid_diff)
304 m_edge_types_to_nums[
"LC"]++;
305 this->m_just_inserted_lc =
true;
315 template <
class GRAPH_T>
320 parent_t::registerNewEdge(from, to, rel_edge);
322 this->m_graph->insertEdge(from, to, rel_edge);
325 template <
class GRAPH_T>
327 std::set<mrpt::graphs::TNodeID>* nodes_set,
336 nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
338 double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
339 this->m_graph->nodes[cur_nodeID]);
342 nodes_set->insert(nodeID);
348 this->m_graph->getAllNodes(*nodes_set);
354 template <
class GRAPH_T>
356 const std::map<std::string, bool>& events_occurred)
359 parent_t::notifyOfWindowEvents(events_occurred);
362 if (events_occurred.find(
params.keystroke_laser_scans)->second)
364 this->toggleLaserScansVisualization();
370 template <
class GRAPH_T>
374 ASSERTDEBMSG_(this->m_win,
"No CDisplayWindow3D* was provided");
375 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* was provided");
384 if (
params.visualize_laser_scans)
387 obj->setVisibility(!
obj->isVisible());
391 dumpVisibilityErrorMsg(
"visualize_laser_scans");
394 this->m_win->unlockAccess3DScene();
395 this->m_win->forceRepaint();
400 template <
class GRAPH_T>
402 std::map<std::string, int>* edge_types_to_num)
const
406 *edge_types_to_num = m_edge_types_to_nums;
411 template <
class GRAPH_T>
417 this->m_time_logger.enter(
"CICPCriteriaERD::Visuals");
418 parent_t::initializeVisuals();
421 params.has_read_config,
"Configuration parameters aren't loaded yet");
423 this->m_win_observer->registerKeystroke(
424 params.keystroke_laser_scans,
"Toggle LaserScans Visualization");
427 if (
params.ICP_max_distance > 0)
433 obj->setPose(initial_pose);
434 obj->setName(
"ICP_max_distance");
435 obj->setColor_u8(m_search_disk_color);
437 params.ICP_max_distance,
params.ICP_max_distance - 0.1);
440 this->m_win->unlockAccess3DScene();
441 this->m_win->forceRepaint();
445 if (
params.visualize_laser_scans)
450 mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
451 laser_scan_viz->enablePoints(
true);
452 laser_scan_viz->enableLine(
true);
453 laser_scan_viz->enableSurface(
true);
454 laser_scan_viz->setSurfaceColor(
455 m_laser_scans_color.R, m_laser_scans_color.G, m_laser_scans_color.B,
456 m_laser_scans_color.A);
458 laser_scan_viz->setName(
"laser_scan_viz");
460 scene->insert(laser_scan_viz);
461 this->m_win->unlockAccess3DScene();
462 this->m_win->forceRepaint();
466 if (this->m_win && this->m_win_manager &&
params.ICP_max_distance > 0)
468 this->m_win_manager->assignTextMessageParameters(
469 &m_offset_y_search_disk, &m_text_index_search_disk);
471 this->m_win_manager->addTextMessage(
472 5, -m_offset_y_search_disk,
mrpt::format(
"ICP Edges search radius"),
474 m_text_index_search_disk);
477 this->m_time_logger.leave(
"CICPCriteriaERD::Visuals");
480 template <
class GRAPH_T>
484 this->m_time_logger.enter(
"CICPCriteriaERD::Visuals");
488 parent_t::updateVisuals();
491 if (this->m_win &&
params.ICP_max_distance > 0)
498 disk_obj->setPose(this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
500 this->m_win->unlockAccess3DScene();
501 this->m_win->forceRepaint();
505 if (this->m_win &&
params.visualize_laser_scans &&
506 (m_last_laser_scan2D || m_fake_laser_scan2D))
512 std::dynamic_pointer_cast<CPlanarLaserScan>(
obj);
515 if (m_fake_laser_scan2D)
517 laser_scan_viz->setScan(*m_fake_laser_scan2D);
521 laser_scan_viz->setScan(*m_last_laser_scan2D);
526 this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
527 if (search != this->m_graph->nodes.end())
529 laser_scan_viz->setPose(
530 this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
533 laser_scan_viz->setPose(
535 laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(),
536 -0.15,
DEG2RAD(laser_scan_viz->getPoseYaw()),
537 DEG2RAD(laser_scan_viz->getPosePitch()),
538 DEG2RAD(laser_scan_viz->getPoseRoll())));
541 this->m_win->unlockAccess3DScene();
542 this->m_win->forceRepaint();
545 this->m_time_logger.leave(
"CICPCriteriaERD::Visuals");
549 template <
class GRAPH_T>
554 using namespace mrpt;
557 "Cannot toggle visibility of specified object.\n "
558 "Make sure that the corresponding visualization flag ( %s "
559 ") is set to true in the .ini file.\n",
561 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
566 template <
class GRAPH_T>
570 parent_t::loadParams(source_fname);
572 params.loadFromConfigFileName(
573 source_fname,
"EdgeRegistrationDeciderParameters");
578 int min_verbosity_level =
source.read_int(
579 "EdgeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
584 template <
class GRAPH_T>
588 parent_t::printParams();
594 template <
class GRAPH_T>
605 stringstream class_props_ss;
606 class_props_ss <<
"ICP Goodness-based Registration Procedure Summary: "
608 class_props_ss << header_sep << std::endl;
611 const std::string time_res = this->m_time_logger.getStatsAsText();
612 const std::string output_res = this->getLogAsString();
616 parent_t::getDescriptiveReport(report_str);
618 *report_str += class_props_ss.str();
619 *report_str += report_sep;
621 *report_str += time_res;
622 *report_str += report_sep;
624 *report_str += output_res;
625 *report_str += report_sep;
633 template <
class GRAPH_T>
635 : decider(d), keystroke_laser_scans(
"l"), has_read_config(false)
639 template <
class GRAPH_T>
644 template <
class GRAPH_T>
646 std::ostream& out)
const
651 "------------------[ Goodness-based ICP Edge Registration "
652 "]------------------\n");
654 "ICP goodness threshold = %.2f%% \n",
655 ICP_goodness_thresh * 100);
657 "ICP max radius for edge search = %.2f\n", ICP_max_distance);
659 "Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
661 "Visualize laser scans = %d\n", visualize_laser_scans);
663 "3DScans Image Directory = %s\n",
664 scans_img_external_dir.c_str());
668 template <
class GRAPH_T>
674 LC_min_nodeid_diff =
source.read_int(
675 "GeneralConfiguration",
"LC_min_nodeid_diff", 30,
false);
677 source.read_double(section,
"ICP_max_distance", 10,
false);
678 ICP_goodness_thresh =
679 source.read_double(section,
"ICP_goodness_thresh", 0.75,
false);
680 visualize_laser_scans =
source.read_bool(
681 "VisualizationParameters",
"visualize_laser_scans",
true,
false);
682 scans_img_external_dir =
source.read_string(
683 section,
"scan_images_external_directory",
"./",
false);
685 has_read_config =
true;