gridslamprocessor_tree.cpp
Go to the documentation of this file.
00001 #include <string>
00002 #include <deque>
00003 #include <list>
00004 #include <map>
00005 #include <set>
00006 #include <fstream>
00007 //#include <gsl/gsl_blas.h>
00008 
00009 #include <gmapping/utils/stat.h>
00010 #include <gmapping/gridfastslam/gridslamprocessor.h>
00011 
00012 namespace GMapping {
00013 
00014 using namespace std;
00015 
00016 GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c){
00017         pose=p;
00018         weight=w;
00019         childs=c;
00020         parent=n;
00021         reading=0;
00022         gweight=0;
00023         if (n){
00024                 n->childs++;
00025         }
00026         flag=0;
00027         accWeight=0;
00028 }
00029 
00030 
00031 GridSlamProcessor::TNode::~TNode(){
00032         if (parent && (--parent->childs)<=0)
00033                 delete parent;
00034         assert(!childs);
00035 }
00036 
00037 
00038 //BEGIN State Save/Restore
00039 
00040 GridSlamProcessor::TNodeVector GridSlamProcessor::getTrajectories() const{
00041   TNodeVector v;
00042   TNodeMultimap parentCache;
00043   TNodeDeque border;
00044         
00045         for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00046                 TNode* node=it->node;
00047                 while(node){
00048                         node->flag=false;
00049                         node=node->parent;
00050                 }
00051         }
00052         
00053         for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00054                 TNode* newnode=new TNode(* (it->node) );
00055                 
00056                 v.push_back(newnode);
00057                 assert(newnode->childs==0);
00058                 if (newnode->parent){
00059                         parentCache.insert(make_pair(newnode->parent, newnode));
00060                         //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
00061                         if (! newnode->parent->flag){
00062                                 //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
00063                                 newnode->parent->flag=true;
00064                                 border.push_back(newnode->parent);
00065                         }
00066                 }
00067         }
00068         
00069         //cerr << __PRETTY_FUNCTION__ << ": border.size(INITIAL)=" << border.size() << endl;
00070         //cerr << __PRETTY_FUNCTION__ << ": parentCache.size()=" << parentCache.size() << endl;
00071         while (! border.empty()){
00072                 //cerr << __PRETTY_FUNCTION__ << ": border.size(PREPROCESS)=" << border.size() << endl;
00073                 //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl;
00074                 const TNode* node=border.front();
00075                 //cerr << __PRETTY_FUNCTION__ << ": node " << node << endl;
00076                 border.pop_front();
00077                 if (! node)
00078                         continue;
00079                         
00080                 TNode* newnode=new TNode(*node);
00081                 node->flag=false;
00082                 
00083                 //update the parent of all of the referring childs 
00084                 pair<TNodeMultimap::iterator, TNodeMultimap::iterator> p=parentCache.equal_range(node);
00085                 double childs=0;
00086                 for (TNodeMultimap::iterator it=p.first; it!=p.second; it++){
00087                         assert(it->second->parent==it->first);
00088                         (it->second)->parent=newnode;
00089                         //cerr << "PS(" << it->first << ", "<< it->second << ")";
00090                         childs++;
00091                 }
00093                 parentCache.erase(p.first, p.second);
00094                 //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl;
00095                 assert(childs==newnode->childs);
00096                 
00097                 //unmark the node
00098                 if ( node->parent ){
00099                         parentCache.insert(make_pair(node->parent, newnode));
00100                         if(! node->parent->flag){
00101                                 border.push_back(node->parent);
00102                                 node->parent->flag=true;
00103                         }       
00104                 }
00105                 //insert the parent in the cache
00106         }
00107         //cerr << __PRETTY_FUNCTION__ << " : checking cloned trajectories" << endl;
00108         for (unsigned int i=0; i<v.size(); i++){
00109                 TNode* node= v[i];
00110                 while (node){
00111                         //cerr <<".";
00112                         node=node->parent;
00113                 }
00114                 //cerr << endl;
00115         }       
00116         
00117         return v;
00118 
00119 }
00120 
00121 void GridSlamProcessor::integrateScanSequence(GridSlamProcessor::TNode* node){
00122         //reverse the list 
00123         TNode* aux=node;
00124         TNode* reversed=0;
00125         double count=0;
00126         while(aux!=0){
00127                 TNode * newnode=new TNode(*aux);
00128                 newnode->parent=reversed;
00129                 reversed=newnode;
00130                 aux=aux->parent;
00131                 count++;
00132         }
00133         
00134         //attach the path to each particle and compute the map;
00135         if (m_infoStream )
00136                 m_infoStream << "Restoring State Nodes=" <<count << endl;
00137                 
00138                 
00139         aux=reversed;
00140         bool first=true;
00141         double oldWeight=0;
00142         OrientedPoint oldPose;
00143         while (aux!=0){
00144                 if (first){
00145                         oldPose=aux->pose;
00146                         first=false;
00147                         oldWeight=aux->weight;
00148                 }
00149                 
00150                 OrientedPoint dp=aux->pose-oldPose;
00151                 double dw=aux->weight-oldWeight;
00152                 oldPose=aux->pose;
00153                 
00154                 
00155                 double * plainReading = new double[m_beams];
00156                 for(unsigned int i=0; i<m_beams; i++)
00157                         plainReading[i]=(*(aux->reading))[i];
00158                 
00159                 for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00160                         //compute the position relative to the path;
00161                         double s=sin(oldPose.theta-it->pose.theta),
00162                                c=cos(oldPose.theta-it->pose.theta);
00163                         
00164                         it->pose.x+=c*dp.x-s*dp.y;
00165                         it->pose.y+=s*dp.x+c*dp.y;
00166                         it->pose.theta+=dp.theta;
00167                         it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta));
00168                         
00169                         //register the scan
00170                         m_matcher.invalidateActiveArea();
00171                         m_matcher.computeActiveArea(it->map, it->pose, plainReading);
00172                         it->weight+=dw;
00173                         it->weightSum+=dw;
00174 
00175                         // this should not work, since it->weight is not the correct weight!
00176                         //                      it->node=new TNode(it->pose, it->weight, it->node);
00177                         it->node=new TNode(it->pose, 0.0, it->node);
00178                         //update the weight
00179                 }
00180                 
00181                 delete [] plainReading;
00182                 aux=aux->parent;
00183         }
00184         
00185         //destroy the path
00186         aux=reversed;
00187         while (reversed){
00188                 aux=reversed;
00189                 reversed=reversed->parent;
00190                 delete aux;
00191         }
00192 }
00193 
00194 //END State Save/Restore
00195 
00196 //BEGIN
00197 
00198 void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){
00199 
00200   if (!weightsAlreadyNormalized) {
00201     normalize();
00202   }
00203   resetTree();
00204   propagateWeights();
00205 }
00206 
00207 void GridSlamProcessor::resetTree(){
00208   // don't calls this function directly, use updateTreeWeights(..) !
00209 
00210         for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00211                 TNode* n=it->node;
00212                 while (n){
00213                         n->accWeight=0;
00214                         n->visitCounter=0;
00215                         n=n->parent;
00216                 }
00217         }
00218 }
00219 
00220 double propagateWeight(GridSlamProcessor::TNode* n, double weight){
00221         if (!n)
00222                 return weight;
00223         double w=0;
00224         n->visitCounter++;
00225         n->accWeight+=weight;
00226         if (n->visitCounter==n->childs){
00227                 w=propagateWeight(n->parent,n->accWeight);
00228         }
00229         assert(n->visitCounter<=n->childs);
00230         return w;
00231 }
00232 
00233 double GridSlamProcessor::propagateWeights(){
00234   // don't calls this function directly, use updateTreeWeights(..) !
00235 
00236         // all nodes must be resetted to zero and weights normalized
00237 
00238         // the accumulated weight of the root
00239         double lastNodeWeight=0;
00240         // sum of the weights in the leafs
00241         double aw=0;                   
00242 
00243         std::vector<double>::iterator w=m_weights.begin();
00244         for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
00245                 double weight=*w;
00246                 aw+=weight;
00247                 TNode * n=it->node;
00248                 n->accWeight=weight;
00249                 lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
00250                 w++;
00251         }
00252         
00253         if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) {
00254           cerr << "ERROR: ";
00255           cerr << "root->accWeight=" << lastNodeWeight << "    sum_leaf_weights=" << aw << endl;
00256           assert(0);         
00257         }
00258         return lastNodeWeight;
00259 }
00260 
00261 };
00262 
00263 //END


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13