00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <assert.h>
00018 #include <queue>
00019 #include <iostream>
00020 #include <iomanip>
00021
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
00109
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 }