19 template <
class GRAPH_T>
28 template <
class GRAPH_T>
30 std::string* report_str)
const
35 parent_t::getDescriptiveReport(report_str);
37 ss <<
"Node Registration Decider Strategy [NRD]: " << endl;
38 *report_str += ss.str();
43 template <
class GRAPH_T>
49 template <
class GRAPH_T>
51 const typename GRAPH_T::constraint_t& constraint)
62 global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
63 this->addNodeAnnotsToPose(&tmp_pose);
67 std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
69 this->m_graph->nodes.insert(
70 make_pair(this->m_graph->root, tmp_pose));
73 "nodeID \"%lu\" with pose \"%s\" seems to be "
74 "already registered.",
75 this->m_graph->root, tmp_pose.asString().c_str()));
77 this->m_prev_registered_nodeID = this->m_graph->root;
81 TNodeID from = this->m_prev_registered_nodeID;
84 TNodeID to = this->m_graph->nodeCount();
88 global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
89 this->addNodeAnnotsToPose(&tmp_pose);
92 std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
94 this->m_graph->nodes.insert(make_pair(to, tmp_pose));
97 "nodeID \"%lu\" with pose \"%s\" seems to be "
98 "already registered.",
99 to, tmp_pose.asString().c_str()));
100 this->m_graph->insertEdgeAtEnd(from, to, constraint);
103 m_prev_registered_nodeID = to;
106 "Registered new node:" << endl
107 <<
"\t" << from <<
" => " << to << endl
109 << constraint.getMeanVal().asString());
115 template <
class GRAPH_T>
118 bool res = this->registerNewNodeAtEnd(this->m_since_prev_node_PDF);
121 this->resetPDF(&m_since_prev_node_PDF);
126 template <
class GRAPH_T>
134 c->cov_inv = this->m_init_inf_mat;
139 template <
class GRAPH_T>
145 template <
class GRAPH_T>
146 typename GRAPH_T::global_pose_t
153 pose_out = this->m_graph->nodes.at(this->m_prev_registered_nodeID);
156 pose_out += m_since_prev_node_PDF.getMeanVal();