10 #ifndef CICPCRITERIAERD_IMPL_H 11 #define CICPCRITERIAERD_IMPL_H 19 template <
class GRAPH_T>
22 m_search_disk_color(142, 142, 56),
23 m_laser_scans_color(0, 20, 255),
24 m_is_using_3DScan(false)
41 template <
class GRAPH_T>
49 template <
class GRAPH_T>
60 bool registered_new_node =
false;
62 if (this->m_last_total_num_nodes < this->m_graph->nodeCount())
64 registered_new_node =
true;
65 this->m_last_total_num_nodes = this->m_graph->nodeCount();
76 m_is_using_3DScan =
false;
86 m_last_laser_scan3D->
load();
89 this->convert3DTo2DRangeScan(
91 &m_fake_laser_scan2D);
93 m_is_using_3DScan =
true;
98 if (registered_new_node)
100 if (m_last_laser_scan2D)
102 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
106 "Added laser scans of nodeID: %u",
107 (
unsigned)(this->m_graph->nodeCount() - 1)));
109 if (m_last_laser_scan3D)
111 m_nodes_to_laser_scans3D[this->m_graph->nodeCount() - 1] =
115 "Added laser scans of nodeID: %u",
116 (
unsigned)(this->m_graph->nodeCount() - 1)));
123 m_last_laser_scan2D =
125 if (registered_new_node && m_last_laser_scan2D)
127 this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
133 if (registered_new_node)
136 std::set<mrpt::graphs::TNodeID> nodes_to_check_ICP;
137 this->getNearbyNodesOf(
138 &nodes_to_check_ICP, this->m_graph->nodeCount() - 1,
141 "Found * %lu * nodes close to nodeID %lu",
142 nodes_to_check_ICP.size(), this->m_graph->nodeCount() - 1);
145 this->m_just_inserted_lc =
false;
146 registered_new_node =
false;
148 if (m_is_using_3DScan)
150 checkRegistrationCondition3D(nodes_to_check_ICP);
154 checkRegistrationCondition2D(nodes_to_check_ICP);
162 template <
class GRAPH_T>
164 const std::set<mrpt::graphs::TNodeID>& nodes_set)
167 using namespace mrpt;
176 search = this->m_nodes_to_laser_scans2D.find(curr_nodeID);
177 if (search != this->m_nodes_to_laser_scans2D.end())
179 curr_laser_scan = search->second;
188 node_it != nodes_set.end(); ++node_it)
196 search = this->m_nodes_to_laser_scans2D.find(*node_it);
197 if (search != this->m_nodes_to_laser_scans2D.end())
199 prev_laser_scan = search->second;
202 pose_t initial_pose = this->m_graph->nodes[curr_nodeID] -
203 this->m_graph->nodes[*node_it];
205 this->m_time_logger.enter(
"CICPCriteriaERD::getICPEdge");
207 *prev_laser_scan, *curr_laser_scan, &rel_edge,
208 &initial_pose, &icp_info);
209 this->m_time_logger.leave(
"CICPCriteriaERD::getICPEdge");
213 ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" 216 "ICP constraint between NON-successive nodes: " 217 << *node_it <<
" => " << curr_nodeID << std::endl
219 <<
"\tgoodness = " << icp_info.
goodness);
221 "ICP_goodness_thresh: " <<
params.ICP_goodness_thresh);
223 "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" 229 this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
230 m_edge_types_to_nums[
"ICP2D"]++;
232 if (
absDiff(curr_nodeID, *node_it) >
233 params.LC_min_nodeid_diff)
235 m_edge_types_to_nums[
"LC"]++;
236 this->m_just_inserted_lc =
true;
245 template <
class GRAPH_T>
247 const std::set<mrpt::graphs::TNodeID>& nodes_set)
259 search = m_nodes_to_laser_scans3D.find(curr_nodeID);
260 if (search != m_nodes_to_laser_scans3D.end())
262 curr_laser_scan = search->second;
271 node_it != nodes_set.end(); ++node_it)
279 search = m_nodes_to_laser_scans3D.find(*node_it);
280 if (search != m_nodes_to_laser_scans3D.end())
282 prev_laser_scan = search->second;
285 this->m_time_logger.enter(
"CICPCriteriaERD::getICPEdge");
287 *prev_laser_scan, *curr_laser_scan, &rel_edge,
nullptr,
289 this->m_time_logger.leave(
"CICPCriteriaERD::getICPEdge");
294 this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
295 m_edge_types_to_nums[
"ICP3D"]++;
297 if (
absDiff(curr_nodeID, *node_it) >
298 params.LC_min_nodeid_diff)
300 m_edge_types_to_nums[
"LC"]++;
301 this->m_just_inserted_lc =
true;
311 template <
class GRAPH_T>
316 parent_t::registerNewEdge(from, to, rel_edge);
318 this->m_graph->insertEdge(from, to, rel_edge);
321 template <
class GRAPH_T>
323 std::set<mrpt::graphs::TNodeID>* nodes_set,
332 nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
334 double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
335 this->m_graph->nodes[cur_nodeID]);
338 nodes_set->insert(nodeID);
344 this->m_graph->getAllNodes(*nodes_set);
350 template <
class GRAPH_T>
352 const std::map<std::string, bool>& events_occurred)
355 parent_t::notifyOfWindowEvents(events_occurred);
358 if (events_occurred.find(
params.keystroke_laser_scans)->second)
360 this->toggleLaserScansVisualization();
366 template <
class GRAPH_T>
370 ASSERTDEBMSG_(this->m_win,
"No CDisplayWindow3D* was provided");
371 ASSERTDEBMSG_(this->m_win_manager,
"No CWindowManager* was provided");
380 if (
params.visualize_laser_scans)
383 obj->setVisibility(!
obj->isVisible());
387 dumpVisibilityErrorMsg(
"visualize_laser_scans");
390 this->m_win->unlockAccess3DScene();
391 this->m_win->forceRepaint();
396 template <
class GRAPH_T>
398 std::map<std::string, int>* edge_types_to_num)
const 402 *edge_types_to_num = m_edge_types_to_nums;
407 template <
class GRAPH_T>
413 this->m_time_logger.enter(
"CICPCriteriaERD::Visuals");
414 parent_t::initializeVisuals();
417 params.has_read_config,
"Configuration parameters aren't loaded yet");
419 this->m_win_observer->registerKeystroke(
420 params.keystroke_laser_scans,
"Toggle LaserScans Visualization");
423 if (
params.ICP_max_distance > 0)
429 obj->setPose(initial_pose);
430 obj->setName(
"ICP_max_distance");
431 obj->setColor_u8(m_search_disk_color);
433 params.ICP_max_distance,
params.ICP_max_distance - 0.1);
436 this->m_win->unlockAccess3DScene();
437 this->m_win->forceRepaint();
441 if (
params.visualize_laser_scans)
446 mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
447 laser_scan_viz->enablePoints(
true);
448 laser_scan_viz->enableLine(
true);
449 laser_scan_viz->enableSurface(
true);
450 laser_scan_viz->setSurfaceColor(
451 m_laser_scans_color.R, m_laser_scans_color.G, m_laser_scans_color.B,
452 m_laser_scans_color.A);
454 laser_scan_viz->setName(
"laser_scan_viz");
456 scene->insert(laser_scan_viz);
457 this->m_win->unlockAccess3DScene();
458 this->m_win->forceRepaint();
462 if (this->m_win && this->m_win_manager &&
params.ICP_max_distance > 0)
464 this->m_win_manager->assignTextMessageParameters(
465 &m_offset_y_search_disk, &m_text_index_search_disk);
467 this->m_win_manager->addTextMessage(
468 5, -m_offset_y_search_disk,
mrpt::format(
"ICP Edges search radius"),
470 m_text_index_search_disk);
473 this->m_time_logger.leave(
"CICPCriteriaERD::Visuals");
476 template <
class GRAPH_T>
480 this->m_time_logger.enter(
"CICPCriteriaERD::Visuals");
484 parent_t::updateVisuals();
487 if (this->m_win &&
params.ICP_max_distance > 0)
494 disk_obj->setPose(this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
496 this->m_win->unlockAccess3DScene();
497 this->m_win->forceRepaint();
501 if (this->m_win &&
params.visualize_laser_scans &&
502 (m_last_laser_scan2D || m_fake_laser_scan2D))
511 if (m_fake_laser_scan2D)
513 laser_scan_viz->setScan(*m_fake_laser_scan2D);
517 laser_scan_viz->setScan(*m_last_laser_scan2D);
522 this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
523 if (search != this->m_graph->nodes.end())
525 laser_scan_viz->setPose(
526 this->m_graph->nodes[this->m_graph->nodeCount() - 1]);
529 laser_scan_viz->setPose(
531 laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(),
532 -0.15,
DEG2RAD(laser_scan_viz->getPoseYaw()),
533 DEG2RAD(laser_scan_viz->getPosePitch()),
534 DEG2RAD(laser_scan_viz->getPoseRoll())));
537 this->m_win->unlockAccess3DScene();
538 this->m_win->forceRepaint();
541 this->m_time_logger.leave(
"CICPCriteriaERD::Visuals");
545 template <
class GRAPH_T>
550 using namespace mrpt;
553 "Cannot toggle visibility of specified object.\n " 554 "Make sure that the corresponding visualization flag ( %s " 555 ") is set to true in the .ini file.\n",
557 std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
562 template <
class GRAPH_T>
566 parent_t::loadParams(source_fname);
568 params.loadFromConfigFileName(
569 source_fname,
"EdgeRegistrationDeciderParameters");
574 int min_verbosity_level =
source.read_int(
575 "EdgeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
580 template <
class GRAPH_T>
584 parent_t::printParams();
590 template <
class GRAPH_T>
601 stringstream class_props_ss;
602 class_props_ss <<
"ICP Goodness-based Registration Procedure Summary: " 604 class_props_ss << header_sep << std::endl;
607 const std::string time_res = this->m_time_logger.getStatsAsText();
608 const std::string output_res = this->getLogAsString();
612 parent_t::getDescriptiveReport(report_str);
614 *report_str += class_props_ss.str();
615 *report_str += report_sep;
617 *report_str += time_res;
618 *report_str += report_sep;
620 *report_str += output_res;
621 *report_str += report_sep;
629 template <
class GRAPH_T>
631 : decider(d), keystroke_laser_scans(
"l"), has_read_config(false)
635 template <
class GRAPH_T>
640 template <
class GRAPH_T>
642 std::ostream& out)
const 647 "------------------[ Goodness-based ICP Edge Registration " 648 "]------------------\n");
650 "ICP goodness threshold = %.2f%% \n",
651 ICP_goodness_thresh * 100);
653 "ICP max radius for edge search = %.2f\n", ICP_max_distance);
655 "Min. node difference for LC = %lu\n", LC_min_nodeid_diff);
657 "Visualize laser scans = %d\n", visualize_laser_scans);
659 "3DScans Image Directory = %s\n",
660 scans_img_external_dir.c_str());
664 template <
class GRAPH_T>
670 LC_min_nodeid_diff =
source.read_int(
671 "GeneralConfiguration",
"LC_min_nodeid_diff", 30,
false);
673 source.read_double(section,
"ICP_max_distance", 10,
false);
674 ICP_goodness_thresh =
675 source.read_double(section,
"ICP_goodness_thresh", 0.75,
false);
676 visualize_laser_scans =
source.read_bool(
677 "VisualizationParameters",
"visualize_laser_scans",
true,
false);
678 scans_img_external_dir =
source.read_string(
679 section,
"scan_images_external_directory",
"./",
false);
681 has_read_config =
true;
void checkRegistrationCondition2D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
ICP-based Edge Registration.
VerbosityLevel
Enumeration of available verbosity levels.
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
double DEG2RAD(const double x)
Degrees to radians.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
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.
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Generic method for fetching the incremental action/observation readings from the calling function...
size_t m_last_total_num_nodes
Keep track of the total number of registered nodes since the last time class method was called...
GLsizei GLsizei GLuint * obj
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge)
Register a new constraint/edge in the current graph.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
A planar disk in the XY plane.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
void checkRegistrationCondition3D(const std::set< mrpt::graphs::TNodeID > &nodes_set)
This namespace contains representation of robot actions and observations.
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
unsigned short nIterations
The number of executed iterations until convergence.
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::map< std::string, int > m_edge_types_to_nums
#define ASSERTDEBMSG_(f, __ERROR_MSG)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
A RGB color - floats in the range [0,1].
The ICP algorithm return information.
The namespace for 3D scene representation and rendering.
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
GLsizei GLsizei GLchar * source
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
void getNearbyNodesOf(std::set< mrpt::graphs::TNodeID > *nodes_set, const mrpt::graphs::TNodeID &cur_nodeID, double distance)
Get a list of the nodeIDs whose position is within a certain distance to the specified nodeID...
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
void updateVisuals()
Update the relevant visual features in CDisplayWindow.
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const
Fill the given map with the type of registered edges as well as the corresponding number of registrat...
GLenum const GLfloat * params
const Scalar * const_iterator
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
virtual void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
void loadParams(const std::string &source_fname)
Fetch the latest observation that the current instance received (most probably during a call to the u...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.