Main MRPT website > C++ reference for MRPT 1.9.9
CNodeRegistrationDecider_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CNODEREGISTRATIONDECIDER_IMPL_H
10 #define CNODEREGISTRATIONDECIDER_IMPL_H
11 
12 using namespace mrpt::graphslam::deciders;
13 using namespace std;
14 
15 #include <sstream>
16 
17 // Implementation of classes defined in the CNodeRegistrationDecider class
18 // template.
19 //
20 template <class GRAPH_T>
22  : m_prev_registered_nodeID(INVALID_NODEID)
23 {
24  using namespace mrpt::poses;
25 
26  m_init_inf_mat.unit();
27  m_init_inf_mat *= 10000;
28  resetPDF(&this->m_since_prev_node_PDF);
29 }
30 
31 template <class GRAPH_T>
33 {
34 }
35 
36 template <class GRAPH_T>
38  std::string* report_str) const
39 {
40  MRPT_START;
41 
42  stringstream ss("");
43  parent_t::getDescriptiveReport(report_str);
44 
45  ss << "Node Registration Decider Strategy [NRD]: " << endl;
46  *report_str += ss.str();
47 
48  MRPT_END;
49 }
50 
51 template <class GRAPH_T>
53 {
54  return false;
55 }
56 
57 template <class GRAPH_T>
59  const typename GRAPH_T::constraint_t& constraint)
60 {
61  MRPT_START;
62  using namespace std;
64 
65  // register the initial node if it doesn't exist.
66  // Runs only once.
67  if (this->m_prev_registered_nodeID == INVALID_NODEID)
68  { // root
69  MRPT_LOG_WARN("Registering root node...");
70  global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
71  this->addNodeAnnotsToPose(&tmp_pose);
72 
73  // make sure that this pair hasn't been registered yet.
74  std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
75  this->m_graph->nodes.insert(
76  make_pair(this->m_graph->root, tmp_pose));
78  res.second, mrpt::format(
79  "nodeID \"%lu\" with pose \"%s\" seems to be "
80  "already registered.",
81  this->m_graph->root, tmp_pose.asString().c_str()));
82 
83  this->m_prev_registered_nodeID = this->m_graph->root;
84  }
85 
86  // FROM nodeID
87  TNodeID from = this->m_prev_registered_nodeID;
88  // TO nodeID
89  // In all cases this is going to be ONE AFTER the last registered nodeID
90  TNodeID to = this->m_graph->nodeCount();
91 
92  // add the new pose.
93  {
94  global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
95  this->addNodeAnnotsToPose(&tmp_pose);
96 
97  // make sure that this pair hasn't been registered yet.
98  std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
99  this->m_graph->nodes.insert(make_pair(to, tmp_pose));
101  res.second, mrpt::format(
102  "nodeID \"%lu\" with pose \"%s\" seems to be "
103  "already registered.",
104  to, tmp_pose.asString().c_str()));
105  this->m_graph->insertEdgeAtEnd(from, to, constraint);
106  }
107 
108  m_prev_registered_nodeID = to;
109 
111  "Registered new node:" << endl
112  << "\t" << from << " => " << to << endl
113  << "\tEdge: "
114  << constraint.getMeanVal().asString());
115 
116  return true;
117  MRPT_END;
118 }
119 
120 template <class GRAPH_T>
122 {
123  bool res = this->registerNewNodeAtEnd(this->m_since_prev_node_PDF);
124 
125  // reset the PDF since the last registered node position
126  this->resetPDF(&m_since_prev_node_PDF);
127 
128  return res;
129 }
130 
131 template <class GRAPH_T>
133 {
134  MRPT_START;
135  ASSERTDEB_(c);
136 
137  *c = constraint_t();
138  ASSERTDEB_(c->isInfType());
139  c->cov_inv = this->m_init_inf_mat;
140 
141  MRPT_END;
142 } // end of resetPDF
143 
144 template <class GRAPH_T>
146  global_pose_t* pose) const
147 {
148 }
149 
150 template <class GRAPH_T>
151 typename GRAPH_T::global_pose_t
153 {
154  global_pose_t pose_out;
155 
156  if (this->m_prev_registered_nodeID != INVALID_NODEID)
157  {
158  pose_out = this->m_graph->nodes.at(this->m_prev_registered_nodeID);
159  }
160 
161  pose_out += m_since_prev_node_PDF.getMeanVal();
162  return pose_out;
163 }
164 
165 #endif /* end of include guard: CNODEREGISTRATIONDECIDER_IMPL_H */
#define MRPT_START
Definition: exceptions.h:262
Interface for implementing node registration classes.
STL namespace.
const GLubyte * c
Definition: glext.h:6313
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:208
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
#define MRPT_END
Definition: exceptions.h:266
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
#define MRPT_LOG_WARN(_STRING)
#define INVALID_NODEID
Definition: TNodeID.h:20
GLuint res
Definition: glext.h:7268
typename GRAPH_t ::constraint_t constraint_t
type of graph constraints



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019