CLoopCloserERD_MR_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-2016, 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 
10 #pragma once
11 
12 namespace mrpt { namespace graphslam { namespace deciders {
13 
14 // Ctors, Dtors
15 template<class GRAPH_T>
17  // CLoopCloser Ctor is automatically called.
18 
19  // since this is for MR-SLAM, we do expect more than one node registered
20  // between successive calls
21  this->m_override_registered_nodes_check = true;
22 
23  this->initializeLoggers("CLoopCloserERD_MR");
24 }
25 
26 template<class GRAPH_T>
28  const std::map<
29  TNodeID,
30  mrpt::obs::CObservation2DRangeScan::Ptr>& nodeIDs_to_scans2D) {
31  mr_parent_t::addBatchOfNodeIDsAndScans(nodeIDs_to_scans2D);
32 
33  this->updateMapPartitions(/*full update=*/ true,
34  /* is_first_time_node_reg = */ false);
35 
36 } // end of addBatchOfNodeIDsAndScans
37 
38 template<class GRAPH_T>
40  TNodeID curr_nodeID) {
41  MRPT_START;
42 
43  // Do scan-matching only if I have initially registered curr_nodeID in the
44  // graph.
45  bool is_own = this->m_engine->isOwnNodeID(curr_nodeID);
46  if (is_own) {
47  lc_parent_t::addScanMatchingEdges(curr_nodeID);
48  }
49 
50  MRPT_END;
51 }
52 
53 template<class GRAPH_T>
55  const TNodeID& curr_nodeID,
56  std::set<TNodeID>* nodes_set) {
57  ASSERT_(nodes_set);
58 
59  size_t fetched_nodeIDs = 0;
60  for (int nodeID_i = static_cast<int>(curr_nodeID)-1;
61  (fetched_nodeIDs <= static_cast<size_t>(this->m_laser_params.prev_nodes_for_ICP) &&
62  nodeID_i >= 0);
63  --nodeID_i) {
64  if (this->m_engine->isOwnNodeID(nodeID_i)) {
65  nodes_set->insert(nodeID_i);
66  fetched_nodeIDs++;
67  }
68  }
69 }
70 
71 // Member methods implementations
72 template<class GRAPH_T>
74  mrpt::obs::CActionCollection::Ptr action,
75  mrpt::obs::CSensoryFrame::Ptr observations,
76  mrpt::obs::CObservation::Ptr observation ) {
77 
78  bool success = lc_parent_t::updateState(action, observations, observation);
79 
80  // search for possible edges with the other agent's graph.
81  // TODO
82 
83  return success;
84 }
85 
86 
87 } } } // end of namespaces
88 
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
void addBatchOfNodeIDsAndScans(const std::map< TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > &nodeIDs_to_scans2D)
void fetchNodeIDsForScanMatching(const TNodeID &curr_nodeID, std::set< TNodeID > *nodes_set)


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Thu Jun 6 2019 19:37:48