Main MRPT website > C++ reference
MRPT logo
bfs_visitor.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-2015, 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 srba {
13 
14 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS>
15 template <
16  class KF_VISITOR,
17  class FEAT_VISITOR,
18  class K2K_EDGE_VISITOR,
19  class K2F_EDGE_VISITOR
20  >
21 void RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::bfs_visitor(
22  const TKeyFrameID root_id,
23  const topo_dist_t max_distance,
24  const bool rely_on_prebuilt_spanning_trees,
25  KF_VISITOR & kf_visitor,
26  FEAT_VISITOR & feat_visitor,
27  K2K_EDGE_VISITOR & k2k_edge_visitor,
28  K2F_EDGE_VISITOR & k2f_edge_visitor ) const
29 {
30  using namespace std;
31  set<TLandmarkID> lm_visited;
32  set<const k2k_edge_t*> k2k_visited;
33  set<const k2f_edge_t*> k2f_visited;
34 
35  if (!rely_on_prebuilt_spanning_trees)
36  { // Don't use prebuilt spanning-trees
37 
38  set<TKeyFrameID> kf_visited;
39  queue<TKeyFrameID> pending;
40  map<TKeyFrameID,topo_dist_t> distances;
41 
42  pending.push(root_id);
43  kf_visited.insert(root_id);
44  distances[root_id] = 0;
45 
46  while (!pending.empty())
47  {
48  const TKeyFrameID next_kf = pending.front();
49  pending.pop();
50 
51  const topo_dist_t cur_dist = distances[next_kf];
52 
53  kf_visitor.visit_kf(next_kf,cur_dist);
54 
55  // Get all connections of this node:
56  ASSERTDEB_(next_kf < rba_state.keyframes.size())
57  const keyframe_info & kfi = rba_state.keyframes[next_kf];
58 
59  // Visit all KF2FEAT edges and features themselves:
60  for (size_t i=0;i<kfi.adjacent_k2f_edges.size();i++)
61  {
62  const k2f_edge_t* ed = kfi.adjacent_k2f_edges[i];
63  const TLandmarkID lm_ID = ed->obs.obs.feat_id;
64  if (!lm_visited.count(lm_ID))
65  {
66  if (feat_visitor.visit_filter_feat(lm_ID,cur_dist) )
67  feat_visitor.visit_feat(lm_ID,cur_dist);
68  lm_visited.insert(lm_ID);
69  }
70  if (!k2f_visited.count(ed))
71  {
72  if (k2f_edge_visitor.visit_filter_k2f(next_kf,ed,cur_dist) )
73  k2f_edge_visitor.visit_k2f(next_kf,ed,cur_dist);
74  k2f_visited.insert(ed);
75  }
76  }
77 
78  // Don't explore nearby keyframes if we are already at the maximum distance from root.
79  if (cur_dist>=max_distance) // At most, we should reach cur_dist==max_distance, but just in case use ">="
80  continue;
81 
82  // Visit all KF2KF edges and queue nearby keyframes:
83  for (size_t i=0;i<kfi.adjacent_k2k_edges.size();i++)
84  {
85  const k2k_edge_t* ed = kfi.adjacent_k2k_edges[i];
86  const TKeyFrameID new_kf = getTheOtherFromPair2(next_kf, *ed);
87  if (!kf_visited.count(new_kf))
88  {
89  if (kf_visitor.visit_filter_kf(new_kf,cur_dist) )
90  {
91  pending.push(new_kf);
92  distances[new_kf]=cur_dist+1;
93  }
94  kf_visited.insert(new_kf);
95  }
96  if (!k2k_visited.count(ed))
97  {
98  if (k2k_edge_visitor.visit_filter_k2k(next_kf,new_kf,ed,cur_dist) )
99  k2k_edge_visitor.visit_k2k(next_kf,new_kf,ed,cur_dist);
100  k2k_visited.insert(ed);
101  }
102  }
103  } // end for each "pending"
104  }
105  else
106  { // Use prebuilt spanning-trees
107  const typename rba_problem_state_t::TSpanningTree::TSpanningTreeSym & st_sym = rba_state.spanning_tree.sym;
108 
109  typename rba_problem_state_t::TSpanningTree::next_edge_maps_t::const_iterator it_ste = st_sym.next_edge.find(root_id);
110  if (it_ste == st_sym.next_edge.end())
111  return; // It might be that this is the first node in the graph/subgraph...
112 
113  const std::map<TKeyFrameID,TSpanTreeEntry> & root_ST = it_ste->second;
114 
115  // make a list with all the KFs in the root's ST, + the root itself:
116  std::vector< std::pair<TKeyFrameID,topo_dist_t> > KFs;
117  KFs.reserve(root_ST.size()+1);
118 
119  KFs.push_back( std::pair<TKeyFrameID,topo_dist_t>(root_id, 0 /* distance */) );
120  for (typename std::map<TKeyFrameID,TSpanTreeEntry>::const_iterator it=root_ST.begin();it!=root_ST.end();++it)
121  KFs.push_back( std::pair<TKeyFrameID,topo_dist_t>(it->first,it->second.distance) );
122 
123  // Go thru the list:
124  for (size_t i=0;i<KFs.size();i++)
125  {
126  const TKeyFrameID kf_id = KFs[i].first;
127  const size_t cur_dist = KFs[i].second;
128 
129  // Don't explore nearby keyframes if we are already at the maximum distance from root.
130  if (cur_dist>max_distance)
131  continue;
132 
133  // Visit KF itself:
134  if (kf_visitor.visit_filter_kf(kf_id,cur_dist) )
135  {
136  kf_visitor.visit_kf(kf_id, cur_dist);
137  }
138 
139  // Get all connections of this KF:
140  ASSERTDEB_(kf_id < rba_state.keyframes.size())
141  const keyframe_info & kfi = rba_state.keyframes[kf_id];
142 
143  // Visit all KF2FEAT edges and features themselves:
144  for (size_t i=0;i<kfi.adjacent_k2f_edges.size();i++)
145  {
146  const k2f_edge_t* ed = kfi.adjacent_k2f_edges[i];
147  const TLandmarkID lm_ID = ed->obs.obs.feat_id;
148  if (!lm_visited.count(lm_ID))
149  {
150  if (feat_visitor.visit_filter_feat(lm_ID,cur_dist) )
151  feat_visitor.visit_feat(lm_ID,cur_dist);
152  lm_visited.insert(lm_ID);
153  }
154  if (!k2f_visited.count(ed))
155  {
156  if (k2f_edge_visitor.visit_filter_k2f(kf_id,ed,cur_dist) )
157  k2f_edge_visitor.visit_k2f(kf_id,ed,cur_dist);
158  k2f_visited.insert(ed);
159  }
160  }
161 
162  // Visit all KF2KF edges:
163  for (size_t i=0;i<kfi.adjacent_k2k_edges.size();i++)
164  {
165  const k2k_edge_t* ed = kfi.adjacent_k2k_edges[i];
166  const TKeyFrameID new_kf = getTheOtherFromPair2(kf_id, *ed);
167 
168  if (!k2k_visited.count(ed))
169  {
170  if (k2k_edge_visitor.visit_filter_k2k(kf_id,new_kf,ed,cur_dist) )
171  k2k_edge_visitor.visit_k2k(kf_id,new_kf,ed,cur_dist);
172  k2k_visited.insert(ed);
173  }
174  }
175  } // end for each KF node in the ST of root
176  } // end if-else use STs
177 }
178 
179 
180 } } // end NS
mrpt::srba::rba_joint_parameterization_traits_t::kf_observation_t::obs
obs_traits_t::observation_t obs
Observation data.
Definition: srba_types.h:433
mrpt::srba::observation_traits::observation_t::feat_id
TLandmarkID feat_id
Observed what.
Definition: srba_types.h:150
mrpt::srba::TLandmarkID
uint64_t TLandmarkID
Numeric IDs for landmarks.
Definition: srba_types.h:32
mrpt::srba::rba_joint_parameterization_traits_t::k2f_edge_t::obs
kf_observation_t obs
Definition: srba_types.h:444
std
STL namespace.
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::srba::kf2kf_pose_traits::k2k_edge_t
Keyframe-to-keyframe edge: an unknown of the problem.
Definition: srba_types.h:82
mrpt::srba::topo_dist_t
uint64_t topo_dist_t
Unsigned integral type for topological distances in a graph/tree.
Definition: srba_types.h:33
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
ASSERTDEB_
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: maps/CColouredPointsMap.h:282
mrpt::srba::rba_joint_parameterization_traits_t::keyframe_info
Information per key-frame needed for RBA.
Definition: srba_types.h:453
mrpt::srba::RbaEngine::bfs_visitor
void bfs_visitor(const TKeyFrameID root_id, const topo_dist_t max_distance, const bool rely_on_prebuilt_spanning_trees, KF_VISITOR &kf_visitor, FEAT_VISITOR &feat_visitor, K2K_EDGE_VISITOR &k2k_edge_visitor, K2F_EDGE_VISITOR &k2f_edge_visitor) const
Visits all k2k & k2f edges following a BFS starting at a given starting node and up to a given maximu...
Definition: bfs_visitor.h:21
mrpt::srba::rba_joint_parameterization_traits_t::k2f_edge_t
Keyframe-to-feature edge: observation data stored for each keyframe.
Definition: srba_types.h:442
mrpt::srba::getTheOtherFromPair2
V getTheOtherFromPair2(const V one, const K2K_EDGE &p)
For usage with K2K_EDGE = typename kf2kf_pose_traits::k2k_edge_t.
Definition: srba_types.h:191
mrpt::srba::TKeyFrameID
uint64_t TKeyFrameID
Numeric IDs for key-frames (KFs)
Definition: srba_types.h:31


Page generated by Doxygen 1.8.9.1 for MRPT 1.3.0 SVN: at Sun Sep 13 03:55:12 UTC 2015