Main MRPT website > C++ reference for MRPT 1.5.7
CNodeRegistrationDecider_impl.h
Go to the documentation of this file.
1 #ifndef CNODEREGISTRATIONDECIDER_IMPL_H
2 #define CNODEREGISTRATIONDECIDER_IMPL_H
3 
4 using namespace mrpt::graphslam::deciders;
5 using namespace std;
6 
7 #include <sstream>
8 
9 // Implementation of classes defined in the CNodeRegistrationDecider class
10 // template.
11 //
12 template<class GRAPH_T>
14  m_prev_registered_nodeID(INVALID_NODEID) {
15  using namespace mrpt::poses;
16 
17  m_init_inf_mat.unit();
18  m_init_inf_mat *= 10000;
19  resetPDF(&this->m_since_prev_node_PDF);
20 
21 }
22 
23 template<class GRAPH_T>
25 }
26 
27 template<class GRAPH_T>
29  std::string* report_str) const {
30  MRPT_START;
31 
32  stringstream ss("");
33  parent_t::getDescriptiveReport(report_str);
34 
35  ss << "Node Registration Decider Strategy [NRD]: " << endl;
36  *report_str += ss.str();
37 
38  MRPT_END;
39 }
40 
41 template<class GRAPH_T>
43 
44 template<class GRAPH_T>
46  const typename GRAPH_T::constraint_t& constraint) {
47  MRPT_START;
48  using namespace mrpt::utils;
49  using namespace std;
50 
51  // register the initial node if it doesn't exist.
52  // Runs only once.
53  if (this->m_prev_registered_nodeID == INVALID_NODEID) { // root
54  MRPT_LOG_WARN_STREAM("Registering root node..." << endl);
55  global_pose_t tmp_pose =
56  this->getCurrentRobotPosEstimation();
57  this->addNodeAnnotsToPose(&tmp_pose);
58 
59  // make sure that this pair hasn't been registered yet.
60  std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
61  this->m_graph->nodes.insert(make_pair(
62  this->m_graph->root, tmp_pose));
63  ASSERTMSG_(res.second,
65  "nodeID \"%lu\" with pose \"%s\" seems to be already registered.",
66  this->m_graph->root, tmp_pose.asString().c_str()));
67 
68  this->m_prev_registered_nodeID = this->m_graph->root;
69  }
70 
71  // FROM nodeID
72  TNodeID from = this->m_prev_registered_nodeID;
73  // TO nodeID
74  // In all cases this is going to be ONE AFTER the last registered nodeID
75  TNodeID to = this->m_graph->nodeCount();
76 
77  // add the new pose.
78  {
79  global_pose_t tmp_pose =
80  this->getCurrentRobotPosEstimation();
81  this->addNodeAnnotsToPose(&tmp_pose);
82 
83  // make sure that this pair hasn't been registered yet.
84  std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
85  this->m_graph->nodes.insert(make_pair(
86  to, tmp_pose));
87  ASSERTMSG_(res.second,
89  "nodeID \"%lu\" with pose \"%s\" seems to be already registered.",
90  to, tmp_pose.asString().c_str()));
91  this->m_graph->insertEdgeAtEnd(from, to, constraint);
92  }
93 
94  m_prev_registered_nodeID = to;
95 
96 
97  MRPT_LOG_DEBUG_STREAM("Registered new node:" << endl <<
98  "\t" << from << " => " << to << endl <<
99  "\tEdge: " << constraint.getMeanVal().asString());
100 
101  return true;
102  MRPT_END;
103 }
104 
105 template<class GRAPH_T>
107  bool res = this->registerNewNodeAtEnd(this->m_since_prev_node_PDF);
108 
109  // reset the PDF since the last registered node position
110  this->resetPDF(&m_since_prev_node_PDF);
111 
112  return res;
113 }
114 
115 template<class GRAPH_T>
117  MRPT_START;
118  ASSERT_(c);
119 
120  *c = constraint_t();
121  ASSERT_(c->isInfType());
122  c->cov_inv = this->m_init_inf_mat;
123 
124  MRPT_END;
125 } // end of resetPDF
126 
127 
128 template<class GRAPH_T>
130  global_pose_t* pose) const { }
131 
132 template<class GRAPH_T>
133 typename GRAPH_T::global_pose_t CNodeRegistrationDecider<GRAPH_T>::
135  global_pose_t pose_out;
136 
137  if (this->m_prev_registered_nodeID != INVALID_NODEID) {
138  pose_out =
139  this->m_graph->nodes.at(this->m_prev_registered_nodeID);
140  }
141 
142  pose_out += m_since_prev_node_PDF.getMeanVal();
143  return pose_out;
144 }
145 
146 #endif /* end of include guard: CNODEREGISTRATIONDECIDER_IMPL_H */
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
Interface for implementing node registration classes.
#define INVALID_NODEID
STL namespace.
GRAPH_t ::constraint_t constraint_t
type of graph constraints
#define MRPT_END
const GLubyte * c
Definition: glext.h:5590
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
#define MRPT_START
#define ASSERT_(f)
GLuint res
Definition: glext.h:6298
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
#define ASSERTMSG_(f, __ERROR_MSG)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019