treeoptimizer3.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 
00044 #ifndef _TREEOPTIMIZER3_HH_
00045 #define _TREEOPTIMIZER3_HH_
00046 
00047 #include "posegraph3.hh"
00048 
00049 namespace AISNavigation {
00050 
00052 struct TreeOptimizer3: public TreePoseGraph3{
00053   typedef std::vector<Pose> PoseVector;
00054 
00056   TreeOptimizer3();
00057 
00059   virtual ~TreeOptimizer3();
00060 
00062   void initializeTreeParameters();
00063 
00065   void initializeOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
00066 
00067   void initializeOnlineOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
00068 
00069   void initializeOnlineIterations();
00070 
00072   void iterate(TreePoseGraph3::EdgeSet* eset=0, bool noPreconditioner=false);
00073 
00075   double error(double* mre=0, double* mte=0, double* are=0, double* ate=0, TreePoseGraph3::EdgeSet* eset=0) const;
00076 
00078   double angularError() const;
00079 
00081   double translationalError() const;
00082 
00083   bool restartOnDivergence;
00084 
00085   inline double getRotGain() const {return rotGain;} 
00086 
00088   int iteration;
00089 
00090   double rpFraction;
00091 
00092 protected:
00093 
00096   Transformation getPose(Vertex*v, Vertex* top);
00097 
00100   Rotation getRotation(Vertex*v, Vertex* top);
00101 
00102   void recomputeTransformations(Vertex*v, Vertex* top);
00103 
00104   void recomputeParameters(Vertex*v, Vertex* top);
00105 
00106   void computePreconditioner();
00107 
00108   void propagateErrors(bool usePreconditioner=false);
00109 
00111   double error(const Edge* e) const;
00112 
00114   double loopError(const Edge* e) const;
00115 
00117   double loopRotationalError(const Edge* e) const;
00118   
00120   double translationalError(const Edge* e) const;
00121 
00123   double rotationalError(const Edge* e) const;
00124 
00125   double traslationalError(const Edge* e) const;
00126   
00128   double gamma[2];
00129 
00131   struct PM_t{
00132     double v [2];
00133     inline double& operator[](int i){return v[i];}
00134   };
00135   typedef std::vector< PM_t > PMVector;
00136   PMVector M;
00137 
00139   int mpl;
00140 
00142   std::vector<double> maxRotationalErrors;
00143 
00145   std::vector<double> maxTranslationalErrors;
00146   double rotGain, trasGain;
00147 
00150   virtual void onStepStart(Edge* e);
00151 
00154   virtual void onStepFinished(Edge* e);
00155 
00158   virtual void onIterationStart(int i);
00159 
00163   virtual void onIterationFinished(int iteration);
00164 
00167   virtual void onRestartBegin();
00168 
00170   virtual void onRestartDone();
00171 
00175   virtual bool isDone();
00176   
00177 };
00178 
00179 }; //namespace AISNavigation
00180 
00181 #endif


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