posegraph3d.h
Go to the documentation of this 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 #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       // TODO this is calculated for each iteration if the mean does not change
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     { // convert Transformation3 to the manifold
00127       // create a axis - axis-length representation out of the Quaternion
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 { // limes for nv -> 0
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       // force symmetry of the matrix
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     { // create Quaternion out of the axis - axis-length representation
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         //return Quaternion(0.5*v[3], 0.5*v[4], 0.5*v[5]);
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 } // end namespace
00189 
00190 #endif


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:59