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