posegraph3.hh
Go to the documentation of this file.
00001 /**********************************************************************
00002  *
00003  * This source code is part of the Tree-based Network Optimizer (TORO)
00004  *
00005  *   TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
00006  *                           Slawomir Grzonka and  Wolfram Burgard
00007  *
00008  * TORO is licences under the Common Creative License,
00009  * Attribution-NonCommercial-ShareAlike 3.0
00010  *
00011  * You are free:
00012  *   - to Share - to copy, distribute and transmit the work
00013  *   - to Remix - to adapt the work
00014  *
00015  * Under the following conditions:
00016  *
00017  *   - Attribution. You must attribute the work in the manner specified
00018  *     by the author or licensor (but not in any way that suggests that
00019  *     they endorse you or your use of the work).
00020  *  
00021  *   - Noncommercial. You may not use this work for commercial purposes.
00022  *  
00023  *   - Share Alike. If you alter, transform, or build upon this work,
00024  *     you may distribute the resulting work only under the same or
00025  *     similar license to this one.
00026  *
00027  * Any of the above conditions can be waived if you get permission
00028  * from the copyright holder.  Nothing in this license impairs or
00029  * restricts the author's moral rights.
00030  *
00031  * TORO is distributed in the hope that it will be useful,
00032  * but WITHOUT ANY WARRANTY; without even the implied 
00033  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00034  * PURPOSE.  
00035  **********************************************************************/
00036 
00043 #ifndef _POSEGRAPH3_HH_
00044 #define _POSEGRAPH3_HH_
00045 
00046 #include "posegraph.hh"
00047 #include "transformation3.hh"
00048 #include <iostream>
00049 #include <vector>
00050 
00051 typedef unsigned int uint;
00052 #ifndef M_PI
00053         #define M_PI 3.14159265359
00054 #endif
00055 
00056 namespace AISNavigation {
00057 
00058 
00061 struct TreePoseGraph3: public TreePoseGraph<Operations3D<double> >{
00062   typedef Operations3D<double> Ops;
00063   
00064   typedef Ops::PoseType           Pose;
00065   typedef Ops::RotationType       Rotation;
00066   typedef Ops::TranslationType    Translation;
00067   typedef Ops::TransformationType Transformation;
00068   typedef Ops::CovarianceType     CovarianceMatrix;
00069   typedef Ops::InformationType    InformationMatrix;
00070 
00071 
00076   bool load( const char* filename, bool overrideCovariances=false, bool twoDimensions=false); 
00077 
00079   bool loadEquivalences( const char* filename); 
00080 
00082   bool save( const char* filename);
00083 
00085   bool saveGnuplot( const char* filename);
00086 
00088   void printDepth( std::ostream& os );
00089 
00091   void printWidth( std::ostream& os );
00092 
00094   void printEdgesStat( std::ostream& os);
00095 
00097   void initializeOnTree();
00098   
00100   void recomputeAllTransformations();
00101 
00102   virtual void initializeFromParentEdge(Vertex* v);
00103 
00105   virtual void revertEdgeInfo(Edge* e);
00106 
00110   virtual void collapseEdge(Edge* e);
00111 
00113   int verboseLevel;
00114 
00115 protected:
00116 
00118   struct ParameterPropagator{
00119     inline void perform(TreePoseGraph3::Vertex* v){
00120       if (!v->parent){
00121         v->parameters=TreePoseGraph3::Transformation(0.,0.,0.,0.,0.,0.);
00122         return;
00123       }
00124       v->parameters=v->parent->transformation.inv()*v->transformation;
00125     }
00126   };
00127 
00129   struct TransformationPropagator{
00130     inline void perform(TreePoseGraph3::Vertex* v){
00131       if (!v->parent){
00132         return;
00133       }
00134       v->transformation=v->parent->transformation*v->parameters;
00135     }
00136   };
00137 
00138 };
00139 
00140 
00141 }; //namespace AISNavigation
00142 #endif
00143 
00144 
00145 


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:32