00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef _AIS_POSEGRAPH_3D_HH_
00018 #define _AIS_POSEGRAPH_3D_HH_
00019
00020 #include "posegraph3d_gradient.h"
00021 #include "posegraph.h"
00022 #include "dijkstra.h"
00023 #include "hogman_minimal/math/transformation.h"
00024
00025 #include <fstream>
00026
00027 namespace AISNavigation {
00028
00029 struct PoseGraph3D : public PoseGraph <Transformation3, Matrix6 > {
00030 virtual void load(std::istream& is, bool overrideCovariances=false, std::vector <PoseGraph3D::Edge*> *orderedEdges=0);
00031 virtual void save(std::ostream& os, const Transformation3& offset=Transformation3(), int type=0, bool onlyMarked=false) const;
00032
00033 virtual void loadBinary(std::istream& is, bool overrideCovariances=false, std::vector <PoseGraph3D::Edge*> *orderedEdges=0);
00034 virtual void saveBinary(std::ostream& os, int type=0, bool onlyMarked=false) const;
00035
00036 virtual void saveGnuplot(std::ostream& os, const Transformation3& offset=Transformation3(), bool onlyMarked=false) const;
00037
00038 virtual void visualizeToStream(std::ostream& os) const;
00039
00040 };
00041
00042
00043 template <>
00044 struct MotionJacobian< PoseGraph<Transformation3, Matrix6> >{
00045 Matrix6 state(const Transformation3& t, const Transformation3& movement){
00046 Matrix6 J;
00047 motionJacobianState(J, t.toVector(), movement.toVector());
00048 return J;
00049 }
00050 Matrix6 measurement(const Transformation3& t, const Transformation3& movement){
00051 Matrix6 J;
00052 motionJacobianMeasurement(J, t.toVector(), movement.toVector());
00053 return J;
00054 }
00055 };
00056
00057 template <>
00058 struct TaylorTerms<PoseGraph3D> {
00059 typedef PoseGraph<Transformation3, Matrix6> PG;
00060
00061 void operator()(PG::TransformationType& fij, PG::InformationType& dfij_dxi, PG::InformationType& dfij_dxj, const PG::Edge& e) {
00062 const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00063 const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00064
00065 std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
00066 fij=vi->transformation.inverse()*vj->transformation;
00067 dfij_dxi=Matrix6::eye(1.);
00068 dfij_dxj=Matrix6::eye(1.);
00069 }
00070 };
00071
00072 template < >
00073 struct Gradient <PoseGraph3D>{
00074 typedef PoseGraph3D PG;
00075
00076 void operator()( PG::TransformationVectorType& eij, PG::InformationType& deij_dxi, PG::InformationType& deij_dxj, const PG::Edge& e) {
00077 const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00078 const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00079
00080 std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
00081 eij = (e.mean(false) * (vi->transformation.inverse() * vj->transformation)).toVector();
00082 deij_dxi = Matrix6::eye(1.);
00083 deij_dxj = Matrix6::eye(1.);
00084 }
00085 };
00086
00087 template <>
00088 struct LocalGradient <PoseGraph3D>{
00089 typedef PoseGraph3D PG;
00090 void operator()( PG::TransformationVectorType& eij, PG::InformationType& deij_dxi, PG::InformationType& deij_dxj, const PG::Edge& e) {
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 eij = (e.mean(false) * (vi->transformation.inverse() * vj->transformation)).toVector();
00095 Vector6 emeanEuler = e.mean().toVector();
00096 Vector6 viEuler = vi->transformation.toVector();
00097 Vector6 vjEuler = vj->transformation.toVector();
00098 eulerGradientXi(deij_dxi, emeanEuler, viEuler, vjEuler);
00099 eulerGradientXj(deij_dxj, emeanEuler, viEuler, vjEuler);
00100 }
00101 };
00102
00103 template <>
00104 struct ManifoldGradient <PoseGraph3D> {
00105 typedef PoseGraph3D PG;
00106
00107 void operator()( PG::TransformationVectorType& eij, PG::InformationType& deij_dxi, PG::InformationType& deij_dxj, const PG::Edge& e) {
00108 const PG::Vertex* vi = reinterpret_cast<const PG::Vertex*>(e.from());
00109 const PG::Vertex* vj = reinterpret_cast<const PG::Vertex*>(e.to());
00110
00111 eij = (e.mean(false) * (vi->transformation.inverse() * vj->transformation)).toVector();
00112
00113 Vector6 emeanEuler = e.mean().toVector();
00114 Vector6 viEuler = vi->transformation.toVector();
00115 Vector6 vjEuler = vj->transformation.toVector();
00116 manifoldGradientXi(deij_dxi, emeanEuler, viEuler, vjEuler);
00117 manifoldGradientXj(deij_dxj, emeanEuler, viEuler, vjEuler);
00118 }
00119 };
00120
00121 template <>
00122 struct TransformCovariance<PoseGraph3D> {
00123 typedef PoseGraph3D PG;
00124
00125 static void pose2Manifold(const PG::TransformationType& p, double manifold[6], bool& limes)
00126 {
00127
00128 manifold[0] = p.translation().x();
00129 manifold[1] = p.translation().y();
00130 manifold[2] = p.translation().z();
00131 const Quaternion& q = p.rotation();
00132 double nv = std::sqrt(q.x()*q.x() + q.y()*q.y() + q.z()*q.z());
00133 if (nv > 1e-12) {
00134 double s = 2 * acos(q.w()) / nv;
00135 manifold[3] = s * q.x();
00136 manifold[4] = s * q.y();
00137 manifold[5] = s * q.z();
00138 limes = false;
00139 } else {
00140 manifold[3] = 2 * q.x();
00141 manifold[4] = 2 * q.y();
00142 manifold[5] = 2 * q.z();
00143 limes = true;
00144 }
00145 }
00146
00147 void operator()(PG::InformationType& covariance, const PG::TransformationType& from, const PG::TransformationType& to)
00148 {
00149 Matrix6 J;
00150 propagateJacobianManifold(J, from.toVector(), to.toVector());
00151 covariance = J * covariance * J.transpose();
00152
00153 for (int i = 0; i < covariance.rows(); ++i)
00154 for (int j = i+1; j < covariance.cols(); ++j)
00155 covariance[j][i] = covariance[i][j];
00156 }
00157 };
00158
00159 template <>
00160 struct PoseUpdate<PoseGraph3D> {
00161 typedef PoseGraph3D PG;
00162 static Quaternion manifoldQuat(PG::TransformationVectorType::BaseType* v)
00163 {
00164 double n = std::sqrt(v[3]*v[3] + v[4]*v[4] + v[5]*v[5]);
00165 if (n > 1e-20) {
00166 double nHalf = 0.5 * n;
00167 double s = std::sin(nHalf) / n;
00168 double qw = std::cos(nHalf);
00169 double qx = v[3] * s;
00170 double qy = v[4] * s;
00171 double qz = v[5] * s;
00172 return Quaternion(qx, qy, qz, qw);
00173 } else {
00174
00175 return Quaternion(0, 0, 0, 1);
00176 }
00177 }
00178 void operator()(PG::TransformationType& t, PG::TransformationVectorType::BaseType* update)
00179 {
00180 t.translation()[0] += update[0];
00181 t.translation()[1] += update[1];
00182 t.translation()[2] += update[2];
00183 t.rotation() *= manifoldQuat(update);
00184 }
00185 };
00186
00187
00188 }
00189
00190 #endif