gridslamprocessor_tree.cpp
Go to the documentation of this file.
1 #include <string>
2 #include <deque>
3 #include <list>
4 #include <map>
5 #include <set>
6 #include <fstream>
7 //#include <gsl/gsl_blas.h>
8 
9 #include <gmapping/utils/stat.h>
11 
12 namespace GMapping {
13 
14 using namespace std;
15 
16 GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c){
17  pose=p;
18  weight=w;
19  childs=c;
20  parent=n;
21  reading=0;
22  gweight=0;
23  if (n){
24  n->childs++;
25  }
26  flag=0;
27  accWeight=0;
28 }
29 
30 
32  if (parent && (--parent->childs)<=0)
33  delete parent;
34  assert(!childs);
35 }
36 
37 
38 //BEGIN State Save/Restore
39 
41  TNodeVector v;
42  TNodeMultimap parentCache;
43  TNodeDeque border;
44 
45  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
46  TNode* node=it->node;
47  while(node){
48  node->flag=false;
49  node=node->parent;
50  }
51  }
52 
53  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
54  TNode* newnode=new TNode(* (it->node) );
55 
56  v.push_back(newnode);
57  assert(newnode->childs==0);
58  if (newnode->parent){
59  parentCache.insert(make_pair(newnode->parent, newnode));
60  //cerr << __func__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
61  if (! newnode->parent->flag){
62  //cerr << __func__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
63  newnode->parent->flag=true;
64  border.push_back(newnode->parent);
65  }
66  }
67  }
68 
69  //cerr << __func__ << ": border.size(INITIAL)=" << border.size() << endl;
70  //cerr << __func__ << ": parentCache.size()=" << parentCache.size() << endl;
71  while (! border.empty()){
72  //cerr << __func__ << ": border.size(PREPROCESS)=" << border.size() << endl;
73  //cerr << __func__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl;
74  const TNode* node=border.front();
75  //cerr << __func__ << ": node " << node << endl;
76  border.pop_front();
77  if (! node)
78  continue;
79 
80  TNode* newnode=new TNode(*node);
81  node->flag=false;
82 
83  //update the parent of all of the referring childs
84  pair<TNodeMultimap::iterator, TNodeMultimap::iterator> p=parentCache.equal_range(node);
85  double childs=0;
86  for (TNodeMultimap::iterator it=p.first; it!=p.second; it++){
87  assert(it->second->parent==it->first);
88  (it->second)->parent=newnode;
89  //cerr << "PS(" << it->first << ", "<< it->second << ")";
90  childs++;
91  }
93  parentCache.erase(p.first, p.second);
94  //cerr << __func__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl;
95  assert(childs==newnode->childs);
96 
97  //unmark the node
98  if ( node->parent ){
99  parentCache.insert(make_pair(node->parent, newnode));
100  if(! node->parent->flag){
101  border.push_back(node->parent);
102  node->parent->flag=true;
103  }
104  }
105  //insert the parent in the cache
106  }
107  //cerr << __func__ << " : checking cloned trajectories" << endl;
108  for (unsigned int i=0; i<v.size(); i++){
109  TNode* node= v[i];
110  while (node){
111  //cerr <<".";
112  node=node->parent;
113  }
114  //cerr << endl;
115  }
116 
117  return v;
118 
119 }
120 
122  //reverse the list
123  TNode* aux=node;
124  TNode* reversed=0;
125  double count=0;
126  while(aux!=0){
127  TNode * newnode=new TNode(*aux);
128  newnode->parent=reversed;
129  reversed=newnode;
130  aux=aux->parent;
131  count++;
132  }
133 
134  //attach the path to each particle and compute the map;
135  if (m_infoStream )
136  m_infoStream << "Restoring State Nodes=" <<count << endl;
137 
138 
139  aux=reversed;
140  bool first=true;
141  double oldWeight=0;
142  OrientedPoint oldPose;
143  while (aux!=0){
144  if (first){
145  oldPose=aux->pose;
146  first=false;
147  oldWeight=aux->weight;
148  }
149 
150  OrientedPoint dp=aux->pose-oldPose;
151  double dw=aux->weight-oldWeight;
152  oldPose=aux->pose;
153 
154 
155  double * plainReading = new double[m_beams];
156  for(unsigned int i=0; i<m_beams; i++)
157  plainReading[i]=(*(aux->reading))[i];
158 
159  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
160  //compute the position relative to the path;
161  double s=sin(oldPose.theta-it->pose.theta),
162  c=cos(oldPose.theta-it->pose.theta);
163 
164  it->pose.x+=c*dp.x-s*dp.y;
165  it->pose.y+=s*dp.x+c*dp.y;
166  it->pose.theta+=dp.theta;
167  it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta));
168 
169  //register the scan
170  m_matcher.invalidateActiveArea();
171  m_matcher.computeActiveArea(it->map, it->pose, plainReading);
172  it->weight+=dw;
173  it->weightSum+=dw;
174 
175  // this should not work, since it->weight is not the correct weight!
176  // it->node=new TNode(it->pose, it->weight, it->node);
177  it->node=new TNode(it->pose, 0.0, it->node);
178  //update the weight
179  }
180 
181  delete [] plainReading;
182  aux=aux->parent;
183  }
184 
185  //destroy the path
186  aux=reversed;
187  while (reversed){
188  aux=reversed;
189  reversed=reversed->parent;
190  delete aux;
191  }
192 }
193 
194 //END State Save/Restore
195 
196 //BEGIN
197 
198 void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){
199 
200  if (!weightsAlreadyNormalized) {
201  normalize();
202  }
203  resetTree();
204  propagateWeights();
205 }
206 
208  // don't calls this function directly, use updateTreeWeights(..) !
209 
210  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
211  TNode* n=it->node;
212  while (n){
213  n->accWeight=0;
214  n->visitCounter=0;
215  n=n->parent;
216  }
217  }
218 }
219 
221  if (!n)
222  return weight;
223  double w=0;
224  n->visitCounter++;
225  n->accWeight+=weight;
226  if (n->visitCounter==n->childs){
228  }
229  assert(n->visitCounter<=n->childs);
230  return w;
231 }
232 
234  // don't calls this function directly, use updateTreeWeights(..) !
235 
236  // all nodes must be resetted to zero and weights normalized
237 
238  // the accumulated weight of the root
239  double lastNodeWeight=0;
240  // sum of the weights in the leafs
241  double aw=0;
242 
243  std::vector<double>::iterator w=m_weights.begin();
244  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
245  double weight=*w;
246  aw+=weight;
247  TNode * n=it->node;
248  n->accWeight=weight;
249  lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
250  w++;
251  }
252 
253  if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) {
254  cerr << "ERROR: ";
255  cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl;
256  assert(0);
257  }
258  return lastNodeWeight;
259 }
260 
261 };
262 
263 //END
void normalize(const Iterator &begin, const Iterator &end)
std::vector< GridSlamProcessor::TNode * > TNodeVector
TNode(const OrientedPoint &pose, double weight, TNode *parent=0, unsigned int childs=0)
std::deque< GridSlamProcessor::TNode * > TNodeDeque
std::multimap< const GridSlamProcessor::TNode *, GridSlamProcessor::TNode * > TNodeMultimap
unsigned int c
Definition: gfs2stream.cpp:41
#define n
Definition: eig3.cpp:11
double propagateWeight(GridSlamProcessor::TNode *n, double weight)
void updateTreeWeights(bool weightsAlreadyNormalized=false)


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20