1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #ifndef G2O_EDGE_SE3_H_ 28 #define G2O_EDGE_SE3_H_ 29 30 #include "g2o/core/base_binary_edge.h" 31 32 #include "g2o_types_slam3d_api.h" 33 #include "vertex_se3.h" 34 35 namespace g2o { 36 37 /** 38 * \brief Edge between two 3D pose vertices 39 * 40 * The transformation between the two vertices is given as an Isometry3. 41 * If z denotes the measurement, then the error function is given as follows: 42 * z^-1 * (x_i^-1 * x_j) 43 */ 44 class G2O_TYPES_SLAM3D_API EdgeSE3 : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3> { 45 public: 46 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 EdgeSE3(); 48 virtual bool read(std::istream& is); 49 virtual bool write(std::ostream& os) const; 50 51 void computeError(); 52 setMeasurement(const Isometry3 & m)53 virtual void setMeasurement(const Isometry3& m){ 54 _measurement = m; 55 _inverseMeasurement = m.inverse(); 56 } 57 setMeasurementData(const number_t * d)58 virtual bool setMeasurementData(const number_t* d){ 59 Eigen::Map<const Vector7> v(d); 60 setMeasurement(internal::fromVectorQT(v)); 61 return true; 62 } 63 getMeasurementData(number_t * d)64 virtual bool getMeasurementData(number_t* d) const{ 65 Eigen::Map<Vector7> v(d); 66 v = internal::toVectorQT(_measurement); 67 return true; 68 } 69 70 void linearizeOplus(); 71 measurementDimension()72 virtual int measurementDimension() const {return 7;} 73 74 virtual bool setMeasurementFromState() ; 75 initialEstimatePossible(const OptimizableGraph::VertexSet &,OptimizableGraph::Vertex *)76 virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 77 OptimizableGraph::Vertex* /*to*/) { 78 return 1.; 79 } 80 81 virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 82 83 protected: 84 Isometry3 _inverseMeasurement; 85 }; 86 87 /** 88 * \brief Output the pose-pose constraint to Gnuplot data file 89 */ 90 class G2O_TYPES_SLAM3D_API EdgeSE3WriteGnuplotAction: public WriteGnuplotAction { 91 public: 92 EdgeSE3WriteGnuplotAction(); 93 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 94 HyperGraphElementAction::Parameters* params_); 95 }; 96 97 #ifdef G2O_HAVE_OPENGL 98 /** 99 * \brief Visualize a 3D pose-pose constraint 100 */ 101 class G2O_TYPES_SLAM3D_API EdgeSE3DrawAction: public DrawAction{ 102 public: 103 EdgeSE3DrawAction(); 104 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 105 HyperGraphElementAction::Parameters* params_); 106 }; 107 #endif 108 109 } // end namespace 110 #endif 111