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-2014, 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  set<TLandmarkID> lm_visited;
31  set<const k2k_edge_t*> k2k_visited;
32  set<const k2f_edge_t*> k2f_visited;
33 
34  if (!rely_on_prebuilt_spanning_trees)
35  { // Don't use prebuilt spanning-trees
36 
37  set<TKeyFrameID> kf_visited;
38  queue<TKeyFrameID> pending;
39  map<TKeyFrameID,topo_dist_t> distances;
40 
41  pending.push(root_id);
42  kf_visited.insert(root_id);
43  distances[root_id] = 0;
44 
45  while (!pending.empty())
46  {
47  const TKeyFrameID next_kf = pending.front();
48  pending.pop();
49 
50  const topo_dist_t cur_dist = distances[next_kf];
51 
52  kf_visitor.visit_kf(next_kf,cur_dist);
53 
54  // Get all connections of this node:
55  ASSERTDEB_(next_kf < rba_state.keyframes.size())
56  const keyframe_info & kfi = rba_state.keyframes[next_kf];
57 
58  // Visit all KF2FEAT edges and features themselves:
59  for (size_t i=0;i<kfi.adjacent_k2f_edges.size();i++)
60  {
61  const k2f_edge_t* ed = kfi.adjacent_k2f_edges[i];
62  const TLandmarkID lm_ID = ed->obs.obs.feat_id;
63  if (!lm_visited.count(lm_ID))
64  {
65  if (feat_visitor.visit_filter_feat(lm_ID,cur_dist) )
66  feat_visitor.visit_feat(lm_ID,cur_dist);
67  lm_visited.insert(lm_ID);
68  }
69  if (!k2f_visited.count(ed))
70  {
71  if (k2f_edge_visitor.visit_filter_k2f(next_kf,ed,cur_dist) )
72  k2f_edge_visitor.visit_k2f(next_kf,ed,cur_dist);
73  k2f_visited.insert(ed);
74  }
75  }
76 
77  // Don't explore nearby keyframes if we are already at the maximum distance from root.
78  if (cur_dist>=max_distance) // At most, we should reach cur_dist==max_distance, but just in case use ">="
79  continue;
80 
81  // Visit all KF2KF edges and queue nearby keyframes:
82  for (size_t i=0;i<kfi.adjacent_k2k_edges.size();i++)
83  {
84  const k2k_edge_t* ed = kfi.adjacent_k2k_edges[i];
85  const TKeyFrameID new_kf = getTheOtherFromPair2(next_kf, *ed);
86  if (!kf_visited.count(new_kf))
87  {
88  if (kf_visitor.visit_filter_kf(new_kf,cur_dist) )
89  {
90  pending.push(new_kf);
91  distances[new_kf]=cur_dist+1;
92  }
93  kf_visited.insert(new_kf);
94  }
95  if (!k2k_visited.count(ed))
96  {
97  if (k2k_edge_visitor.visit_filter_k2k(next_kf,new_kf,ed,cur_dist) )
98  k2k_edge_visitor.visit_k2k(next_kf,new_kf,ed,cur_dist);
99  k2k_visited.insert(ed);
100  }
101  }
102  } // end for each "pending"
103  }
104  else
105  { // Use prebuilt spanning-trees
106  const typename rba_problem_state_t::TSpanningTree::TSpanningTreeSym & st_sym = rba_state.spanning_tree.sym;
107 
108  typename rba_problem_state_t::TSpanningTree::next_edge_maps_t::const_iterator it_ste = st_sym.next_edge.find(root_id);
109  if (it_ste == st_sym.next_edge.end())
110  return; // It might be that this is the first node in the graph/subgraph...
111 
112  const std::map<TKeyFrameID,TSpanTreeEntry> & root_ST = it_ste->second;
113 
114  // make a list with all the KFs in the root's ST, + the root itself:
115  std::vector< std::pair<TKeyFrameID,topo_dist_t> > KFs;
116  KFs.reserve(root_ST.size()+1);
117 
118  KFs.push_back( std::pair<TKeyFrameID,topo_dist_t>(root_id, 0 /* distance */) );
119  for (typename std::map<TKeyFrameID,TSpanTreeEntry>::const_iterator it=root_ST.begin();it!=root_ST.end();++it)
120  KFs.push_back( std::pair<TKeyFrameID,topo_dist_t>(it->first,it->second.distance) );
121 
122  // Go thru the list:
123  for (size_t i=0;i<KFs.size();i++)
124  {
125  const TKeyFrameID kf_id = KFs[i].first;
126  const size_t cur_dist = KFs[i].second;
127 
128  // Don't explore nearby keyframes if we are already at the maximum distance from root.
129  if (cur_dist>max_distance)
130  continue;
131 
132  // Visit KF itself:
133  if (kf_visitor.visit_filter_kf(kf_id,cur_dist) )
134  {
135  kf_visitor.visit_kf(kf_id, cur_dist);
136  }
137 
138  // Get all connections of this KF:
139  ASSERTDEB_(kf_id < rba_state.keyframes.size())
140  const keyframe_info & kfi = rba_state.keyframes[kf_id];
141 
142  // Visit all KF2FEAT edges and features themselves:
143  for (size_t i=0;i<kfi.adjacent_k2f_edges.size();i++)
144  {
145  const k2f_edge_t* ed = kfi.adjacent_k2f_edges[i];
146  const TLandmarkID lm_ID = ed->obs.obs.feat_id;
147  if (!lm_visited.count(lm_ID))
148  {
149  if (feat_visitor.visit_filter_feat(lm_ID,cur_dist) )
150  feat_visitor.visit_feat(lm_ID,cur_dist);
151  lm_visited.insert(lm_ID);
152  }
153  if (!k2f_visited.count(ed))
154  {
155  if (k2f_edge_visitor.visit_filter_k2f(kf_id,ed,cur_dist) )
156  k2f_edge_visitor.visit_k2f(kf_id,ed,cur_dist);
157  k2f_visited.insert(ed);
158  }
159  }
160 
161  // Visit all KF2KF edges:
162  for (size_t i=0;i<kfi.adjacent_k2k_edges.size();i++)
163  {
164  const k2k_edge_t* ed = kfi.adjacent_k2k_edges[i];
165  const TKeyFrameID new_kf = getTheOtherFromPair2(kf_id, *ed);
166 
167  if (!k2k_visited.count(ed))
168  {
169  if (k2k_edge_visitor.visit_filter_k2k(kf_id,new_kf,ed,cur_dist) )
170  k2k_edge_visitor.visit_k2k(kf_id,new_kf,ed,cur_dist);
171  k2k_visited.insert(ed);
172  }
173  }
174  } // end for each KF node in the ST of root
175  } // end if-else use STs
176 }
177 
178 
179 } } // 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
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: 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.8 for MRPT 1.2.2 SVN:Unversioned directory at Tue Oct 14 02:14:08 UTC 2014