hogman_minimal: hogman_minimal: posegraph.hpp Source File

00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <assert.h>
00018 #include <queue>
00019 #include <iostream>
00020 #include <iomanip>
00021 //#include "posegraph2d.hh"
00022 
00023 #define LINESIZE (4096*4)
00024 
00025 namespace AISNavigation{
00026   using namespace std;
00027 
00028   template <typename T, typename I>
00029   PoseGraph<T,I>::Vertex::Vertex(int i) : Graph::Vertex(i){
00030     _tempIndex=-1;
00031     _isBackup=false;
00032     _fixed=false;
00033   }
00034 
00035   template <typename T, typename I>
00036   PoseGraph<T,I>::Vertex::~Vertex(){
00037   }
00038 
00039   template <typename T, typename I>
00040   PoseGraph<T,I>::Edge::Edge(PoseGraph<T,I>::Vertex* from, PoseGraph<T,I>::Vertex* to, const PoseGraph<T,I>::TransformationType& m, const PoseGraph<T,I>::InformationType& i) : Graph::Edge(from, to){
00041     setAttributes(m,i);
00042   }
00043 
00044   template <typename T, typename I>
00045   bool PoseGraph<T,I>::Edge::direction(Vertex* from_, Vertex* to_) const {
00046     if ((to_==_to)&&(from_==_from))
00047       return true;
00048     if ((to_==_from)&&(from_==_to))
00049       return false;
00050     assert(0);
00051     return false;
00052   }
00053 
00054   template <typename T, typename I>
00055   const typename PoseGraph<T,I>::TransformationType& PoseGraph<T,I>::Edge::mean(bool direct) const {
00056     return direct ? _mean : _rmean;
00057   }
00058 
00059   template <typename T, typename I>
00060   const typename PoseGraph<T,I>::InformationType& PoseGraph<T,I>::Edge::information(bool direct) const {
00061     return direct ? _information :_rinformation;
00062   }
00063 
00064   template <typename T, typename I>
00065   const typename PoseGraph<T,I>::InformationType& PoseGraph<T,I>::Edge::covariance(bool direct) const {
00066     return direct ? _covariance : _rcovariance;
00067   }
00068 
00069   template <typename T, typename I>
00070   const double&    PoseGraph<T,I>::Edge::informationDet(bool direct) const{
00071     return direct ? _infoDet : _rinfoDet;
00072   }
00073 
00074   template <typename T, typename I>
00075   const double&    PoseGraph<T,I>::Edge::covarianceDet(bool direct) const {
00076     return direct ? _covDet : _rcovDet;
00077   }
00078 
00079   template <typename T, typename I>
00080   bool PoseGraph<T,I>::Edge::revert(){
00081     typename PoseGraph<T,I>::TransformationType t_ap(_mean);
00082     _mean=_rmean;
00083     _rmean=t_ap;
00084     typename PoseGraph<T,I>::InformationType i_ap(_information);
00085     _information=_rinformation;
00086     _rinformation=i_ap;
00087     typename PoseGraph<T,I>::InformationType c_ap(_covariance);
00088     _covariance=_rcovariance;
00089     _rcovariance=c_ap;
00090     double id_ap=_infoDet;
00091     _infoDet=_rinfoDet;
00092     _rinfoDet=id_ap;
00093     double ic_ap=_covDet;
00094     _covDet=_rcovDet;
00095     _rcovDet=ic_ap;
00096     return Graph::Edge::revert();
00097   }
00098 
00099   template <typename T, typename I>
00100   void PoseGraph<T,I>::Edge::setAttributes(const PoseGraph<T,I>::TransformationType& mean_, const PoseGraph<T,I>::InformationType& information_){
00101     _mean=mean_;
00102     _information=information_;
00103 
00104     _covariance=information_.inverse();
00105     _infoDet=information_.det();
00106     _covDet=1./_infoDet;
00107     _rmean=mean_.inverse();
00108     //HACK, should use jacobians to project the information matrices and
00109     //the covariances
00110     _rinformation=_information;
00111     _rcovariance=_covariance;
00112     _rinfoDet=_infoDet;
00113     _rcovDet=_covDet;
00114   }
00115 
00116   template <typename T, typename I>
00117   double PoseGraph<T,I>::Edge::chi2() const{
00118     typename PoseGraph<T,I>::Vertex* v1 = reinterpret_cast<typename PoseGraph<T,I>::Vertex*>(_from);
00119     typename PoseGraph<T,I>::Vertex* v2 = reinterpret_cast<typename PoseGraph<T,I>::Vertex*>(_to);
00120     typename PoseGraph<T,I>::TransformationType delta=_rmean * (v1->transformation.inverse()*v2->transformation);
00121     typename PoseGraph<T,I>::TransformationVectorType dp=delta.toVector();
00122     typename PoseGraph<T,I>::TransformationVectorType partial=_information*dp;
00123     return dp*partial;
00124   }
00125 
00126   template <typename T, typename I>
00127   double PoseGraph<T,I>::PathLengthCostFunction::operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to){
00128     const typename PoseGraph<T,I>::Edge* e=dynamic_cast<const typename PoseGraph<T,I>::Edge*>(edge);
00129     typename T::TranslationType t=e->mean().translation();
00130     return sqrt(t*t);
00131   }
00132 
00133   template <typename T, typename I>
00134   double PoseGraph<T,I>::CovarianceDetCostFunction::operator()(Graph::Edge* edge, Graph::Vertex* from, Graph::Vertex* to){
00135     const typename PoseGraph<T,I>::Edge* e=dynamic_cast<const typename PoseGraph<T,I>::Edge*>(edge);
00136     return e->covarianceDet();
00137   }
00138 
00139 
00140   template <typename T, typename I>
00141   typename PoseGraph<T,I>::Vertex* PoseGraph<T,I>::addVertex(const int& k){
00142     Vertex* v=new typename PoseGraph<T,I>::Vertex(k);
00143     Vertex* vresult=dynamic_cast<typename PoseGraph<T,I>::Vertex*>(Graph::addVertex(v));
00144     if (!vresult){
00145       delete v;
00146     }
00147     return vresult;
00148   }
00149 
00150   template <typename T, typename I>
00151   typename PoseGraph<T,I>::Vertex* PoseGraph<T,I>::addVertex(int id, const PoseGraph<T,I>::TransformationType& pose, const PoseGraph<T,I>::InformationType& information){
00152     typename PoseGraph<T,I>::Vertex* v=new typename PoseGraph<T,I>::Vertex(id);
00153     typename PoseGraph<T,I>::Vertex* vresult=dynamic_cast< typename PoseGraph<T,I>::Vertex*>(Graph::addVertex(v));
00154     if (!vresult){
00155       delete v;
00156       return vresult;
00157     }
00158     vresult->transformation=pose;
00159     vresult->covariance=information.inverse();
00160     return vresult;
00161   }
00162 
00163   template <typename T, typename I>
00164   typename PoseGraph<T,I>::Edge*   PoseGraph<T,I>::addEdge(Vertex* from, Vertex* to, const PoseGraph<T,I>::TransformationType& mean, const PoseGraph<T,I>::InformationType& information) {
00165     typename PoseGraph<T,I>::Edge* e=new typename PoseGraph<T,I>::Edge(from, to, mean, information);
00166     typename PoseGraph<T,I>::Edge* eresult=dynamic_cast<typename PoseGraph<T,I>::Edge*>(Graph::addEdge(e));
00167     if (! eresult)
00168       delete e;
00169     return eresult;
00170   }
00171 
00172 
00173   template <typename PG>
00174   struct CovariancePropagator: public Dijkstra::TreeAction{
00175     virtual double perform(Graph::Vertex* v_, Graph::Vertex* vParent_, Graph::Edge* e_){
00176       typename PG::Vertex* v =dynamic_cast<typename PG::Vertex*>(v_);
00177       typename PG::Vertex* vParent =dynamic_cast<typename PG::Vertex*>(vParent_);
00178       typename PG::Edge* e =dynamic_cast<typename PG::Edge*>(e_);
00179       assert(v);
00180       typename PG::InformationType& cov(v->covariance);
00181       if (! vParent){
00182         cov=PG::InformationType::eye(0.);
00183         return 0;
00184       }
00185       assert(vParent);
00186       assert(e);
00187       bool direction=e->direction(vParent, v);
00188       MotionJacobian<PG> jacobian;
00189       typename PG::InformationType Jx=jacobian.state(vParent->transformation, e->mean(direction));
00190       typename PG::InformationType Ju = jacobian.measurement(vParent->transformation, e->mean(direction));
00191       v->covariance=Jx*vParent->covariance*Jx.transpose() + Ju*e->covariance()*Ju.transpose();
00192       return v->covariance.det();
00193     }
00194   };
00195 
00196   template <typename PG>
00197   struct PosePropagator: public Dijkstra::TreeAction{
00198     virtual double perform(Graph::Vertex* v_, Graph::Vertex* vParent_, Graph::Edge* e_){
00199       typename PG::Vertex* v =dynamic_cast<typename PG::Vertex*>(v_);
00200       typename PG::Vertex* vParent =dynamic_cast<typename PG::Vertex*>(vParent_);
00201       typename PG::Edge* e =dynamic_cast<typename PG::Edge*>(e_);
00202       assert(v);
00203       if (v->fixed())
00204         return 1;
00205       if (! vParent){
00206         return 0;
00207       }
00208       assert(vParent);
00209       assert(e);
00210       bool direction=e->direction(vParent, v);
00211       v->transformation=vParent->transformation*e->mean(direction);
00212       return 1;
00213     }
00214   };
00215 
00216 
00217   template <typename T, typename I>
00218   void PoseGraph<T,I>::propagateAlongDijkstraTree(typename PoseGraph<T,I>::Vertex* v, Dijkstra::AdjacencyMap& amap, bool covariance, bool transformation){
00219     Dijkstra::computeTree(v,amap);
00220     if (covariance){
00221       CovariancePropagator<PoseGraph<T,I> > cp;
00222       Dijkstra::visitAdjacencyMap(v,amap,&cp);
00223     }
00224     if (transformation){
00225       PosePropagator< PoseGraph<T,I> > tp;
00226       Dijkstra::visitAdjacencyMap(v,amap,&tp);
00227     }
00228   }
00229 
00230   template <typename T, typename I>
00231   void PoseGraph<T,I>::refineEdge(typename PoseGraph<T,I>::Edge* e, const typename PoseGraph<T,I>::TransformationType& mean, const typename PoseGraph<T,I>::InformationType& information){
00232     e->setAttributes(mean, information);
00233   }
00234 
00235 } // end namespace