00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef POSEGRAPH_2D_HH
00018 #define POSEGRAPH_2D_HH
00019
00020 #include "hogman_minimal/math/transformation.h"
00021
00022 #include "posegraph.h"
00023
00024 namespace AISNavigation{
00025
00026 struct PoseGraph2D: public PoseGraph<Transformation2, Matrix3> {
00027
00028 virtual void load(std::istream& is, bool overrideCovariances=false, std::vector <Edge*> *orderedEdges=0);
00029 virtual void save(std::ostream& os, const Transformation2& offset=Transformation2(), int type=0, bool onlyMarked=false) const;
00030
00031 virtual void visualizeToStream(std::ostream& os) const;
00032
00033 virtual void saveAsGnuplot(ostream& os, bool onlyMarked=false) const;
00034
00035 };
00036
00037 template <>
00038 struct MotionJacobian< PoseGraph<Transformation2, Matrix3 > >{
00039 Matrix3 state(const Transformation2& t, const Transformation2& movement){
00040 Matrix3 j;
00041 double dx=movement.translation().x();
00042 double dy=movement.translation().y();
00043 double theta=t.rotation();
00044 double s=sin(theta), c=cos(theta);
00045 j[0][0]=1.; j[0][1]=0.; j[0][2]=-s*dx-c*dy;
00046 j[1][0]=0.; j[1][1]=1.; j[1][2]=+c*dx-s*dy;
00047 j[2][0]=0.; j[2][1]=0.; j[2][2]=1;
00048 return j;
00049 }
00050
00051 Matrix3 measurement(const Transformation2& t, const Transformation2& movement __attribute__((unused))){
00052 Matrix3 Ju;
00053 double s=sin(t.rotation()), c=cos(t.rotation());
00054 Ju[0][0]=c; Ju[0][1]=-s; Ju[0][2]=0;
00055 Ju[1][0]=s; Ju[1][1]=c; Ju[1][2]=0;
00056 Ju[2][0]=0.; Ju[2][1]=0.; Ju[2][2]=1;
00057 return Ju;
00058 }
00059 };
00060
00061 template <>
00062 struct TaylorTerms< PoseGraph2D > {
00063 typedef PoseGraph2D PG;
00064
00065 void operator()(PG::TransformationType& fij, PG::InformationType& dfij_dxi, PG::InformationType& dfij_dxj, const PG::Edge& e) {
00066 const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00067 const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00068
00069 fij=vi->transformation.inverse()*vj->transformation;
00070 double thetai=vi->transformation.rotation();
00071 Vector2 dt=vj->transformation.translation()-vi->transformation.translation();
00072 double si=sin(thetai), ci=cos(thetai);
00073
00074 dfij_dxi[0][0]=-ci; dfij_dxi[0][1]=-si; dfij_dxi[0][2]= -si*dt.x()+ci*dt.y();
00075 dfij_dxi[1][0]= si; dfij_dxi[1][1]=-ci; dfij_dxi[1][2]= -ci*dt.x()-si*dt.y();
00076 dfij_dxi[2][0]= 0; dfij_dxi[2][1]=0; dfij_dxi[2][2]= -1;
00077
00078 dfij_dxj[0][0]= ci; dfij_dxj[0][1]= si; dfij_dxj[0][2]= 0;
00079 dfij_dxj[1][0]=-si; dfij_dxj[1][1]= ci; dfij_dxj[1][2]= 0;
00080 dfij_dxj[2][0]= 0; dfij_dxj[2][1]=0; dfij_dxj[2][2]= 1;
00081
00082 }
00083 };
00084
00085 template < >
00086 struct Gradient < PoseGraph2D >{
00087 typedef PoseGraph2D PG;
00088
00089 void operator()( PG::TransformationVectorType& eij, PG::InformationType& deij_dxi, PG::InformationType& deij_dxj, const PG::Edge& e) {
00090
00091 const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00092 const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00093
00094 PG::TransformationType Tj=vi->transformation*e.mean();
00095 eij=vj->transformation.toVector();
00096
00097 PG::TransformationVectorType pj=Tj.toVector();
00098 eij-=pj;
00099
00100 double thetai=vi->transformation.rotation();
00101 Vector2 dt=e.mean().translation();
00102 double si=sin(thetai), ci=cos(thetai);
00103
00104 deij_dxi[0][0]=-1; deij_dxi[0][1]= 0; deij_dxi[0][2]= -si*dt.x()+ci*dt.y();
00105 deij_dxi[1][0]= 0; deij_dxi[1][1]=-1; deij_dxi[1][2]= -ci*dt.x()-si*dt.y();
00106 deij_dxi[2][0]= 0; deij_dxi[2][1]=0; deij_dxi[2][2]= -1;
00107
00108 deij_dxj[0][0]= 1; deij_dxj[0][1]= 0; deij_dxj[0][2]= 0;
00109 deij_dxj[1][0]= 0; deij_dxj[1][1]= 1; deij_dxj[1][2]= 0;
00110 deij_dxj[2][0]= 0; deij_dxj[2][1]=0; deij_dxj[2][2]= 1;
00111 }
00112
00113 };
00114
00115 template <>
00116 struct LocalGradient < PoseGraph2D >{
00117 typedef PoseGraph2D PG;
00118
00119 void operator()( PG::TransformationVectorType& eij, PG::InformationType& deij_dxi, PG::InformationType& deij_dxj, const PG::Edge& e) {
00120 TaylorTerms<PG> taylorTerms;
00121 PG::TransformationType fij;
00122 taylorTerms(fij, deij_dxi, deij_dxj, e);
00123 PG::TransformationType rmean=e.mean(false);
00124 eij=(rmean*fij).toVector();
00125 Matrix3 z=rmean.toMatrix();
00126 z[0][2]=0;
00127 z[1][2]=0;
00128 deij_dxi = z * deij_dxi;
00129 deij_dxj = z * deij_dxj;
00130 }
00131 };
00132
00133 template <>
00134 struct ManifoldGradient < PoseGraph2D >{
00135 typedef PoseGraph2D PG;
00136
00137 void operator()( PG::TransformationVectorType& eij, PG::InformationType& deij_dxi, PG::InformationType& deij_dxj, const PG::Edge& e) {
00138 TaylorTerms<PG> taylorTerms;
00139 PG::TransformationType fij;
00140 taylorTerms(fij, deij_dxi, deij_dxj, e);
00141 PG::TransformationType rmean=e.mean(false);
00142 eij=(rmean*fij).toVector();
00143 Matrix3 z=rmean.toMatrix();
00144 z[0][2]=0;
00145 z[1][2]=0;
00146 deij_dxi = z * deij_dxi;
00147 deij_dxj = z * deij_dxj;
00148 }
00149 };
00150
00151 template <>
00152 struct TransformCovariance<PoseGraph2D> {
00153 typedef PoseGraph2D PG;
00154 void operator()(PG::InformationType& covariance, const PG::TransformationType& t, const PG::TransformationType& )
00155 {
00156 PG::InformationType J = t.inverse().toMatrix();
00157 J[0][2]=0.;
00158 J[1][2]=0.;
00159 J[2][0]=0.;
00160 J[2][1]=0.;
00161 J[2][2]=1.;
00162 covariance = J * covariance * J.transpose();
00163 }
00164 };
00165
00166 template <>
00167 struct PoseUpdate<PoseGraph2D> {
00168 typedef PoseGraph2D PG;
00169 void operator()(PG::TransformationType& t, PG::TransformationVectorType::BaseType* update)
00170 {
00171 PG::TransformationVectorType p = t.toVector();
00172 for (int k=0; k<3; k++){
00173 if (k==2 && fabs(update[k])>M_PI){
00174 update[k] = update[k]>0?M_PI:-M_PI;
00175 }
00176 p[k]+=update[k];
00177 }
00178 t = PG::TransformationType::fromVector(p);
00179 }
00180 };
00181
00182 }
00183
00184 #endif
