Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/toro3d/treeoptimizer3.hh

@@ -0,0 +1,181 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file treeoptimizer3.hh
+ *
+ * \brief Defines the core optimizer class for 3D graphs which is a
+ * subclass of TreePoseGraph3
+ *
+ **/
+
+#ifndef _TREEOPTIMIZER3_HH_
+#define _TREEOPTIMIZER3_HH_
+
+#include "posegraph3.hh"
+
+namespace AISNavigation {
+
+/** \brief Class that contains the core optimization algorithm **/
+struct TreeOptimizer3: public TreePoseGraph3{
+  typedef std::vector<Pose> PoseVector;
+
+  /** Constructor **/
+  TreeOptimizer3();
+
+  /** Destructor **/
+  virtual ~TreeOptimizer3();
+
+  /** Initialization function **/
+  void initializeTreeParameters();
+
+  /** Initialization function **/
+  void initializeOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
+
+  void initializeOnlineOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
+
+  void initializeOnlineIterations();
+
+  /** Performs one iteration of the algorithm **/
+  void iterate(TreePoseGraph3::EdgeSet* eset=0, bool noPreconditioner=false);
+
+  /** Conmputes the gloabl error of the network **/
+  double error(double* mre=0, double* mte=0, double* are=0, double* ate=0, TreePoseGraph3::EdgeSet* eset=0) const;
+
+  /** Conmputes the gloabl error of the network **/
+  double angularError() const;
+
+  /** Conmputes the gloabl error of the network **/
+  double translationalError() const;
+
+  bool restartOnDivergence;
+
+  inline double getRotGain() const {return rotGain;} 
+
+  /** Iteration counter **/
+  int iteration;
+
+  double rpFraction;
+
+protected:
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Transformation getPose(Vertex*v, Vertex* top);
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Rotation getRotation(Vertex*v, Vertex* top);
+
+  void recomputeTransformations(Vertex*v, Vertex* top);
+
+  void recomputeParameters(Vertex*v, Vertex* top);
+
+  void computePreconditioner();
+
+  void propagateErrors(bool usePreconditioner=false);
+
+  /** Computes the error of the constraint/edge e **/
+  double error(const Edge* e) const;
+
+  /** Computes the error of the constraint/edge e **/
+  double loopError(const Edge* e) const;
+
+  /** Computes the rotational error of the constraint/edge e **/
+  double loopRotationalError(const Edge* e) const;
+  
+  /** Conmputes the error of the constraint/edge e **/
+  double translationalError(const Edge* e) const;
+
+  /** Conmputes the error of the constraint/edge e **/
+  double rotationalError(const Edge* e) const;
+
+  double traslationalError(const Edge* e) const;
+  
+  /** Used to compute the learning rate lambda **/
+  double gamma[2];
+
+  /** The simplified version of the preconditioning matrix **/
+  struct PM_t{
+    double v [2];
+    inline double& operator[](int i){return v[i];}
+  };
+  typedef std::vector< PM_t > PMVector;
+  PMVector M;
+
+  /**cached maximum path length*/
+  int mpl;
+
+  /**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
+  std::vector<double> maxRotationalErrors;
+
+  /**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
+  std::vector<double> maxTranslationalErrors;
+  double rotGain, trasGain;
+
+  /**callback invoked before starting the optimization of an individual constraint,
+   @param e: the constraint being optimized*/
+  virtual void onStepStart(Edge* e);
+
+  /**callback invoked after finishing the optimization of an individual constraint,
+   @param e: the constraint optimized*/
+  virtual void onStepFinished(Edge* e);
+
+  /**callback invoked before starting a full iteration,
+   @param i: the current iteration number*/
+  virtual void onIterationStart(int i);
+
+  /**callback invoked after finishing a full iteration,
+   @param i: the current iteration number*/
+
+  virtual void onIterationFinished(int iteration);
+
+  /**callback invoked before a restart of the optimizer
+   when the angular wraparound is detected*/
+  virtual void onRestartBegin();
+
+  /**callback invoked after a restart of the optimizer*/
+  virtual void onRestartDone();
+
+  /**callback for determining a termination condition,
+     it can be used by an external thread for stopping the optimizer while performing an iteration.
+     @returns true when the optimizer has to stop.*/
+  virtual bool isDone();
+  
+};
+
+}; //namespace AISNavigation
+
+#endif