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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019