1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_LINE2D_POINTXY_H 28 #define G2O_EDGE_LINE2D_POINTXY_H 29 30 #include "g2o/config.h" 31 #include "g2o/types/slam2d/vertex_point_xy.h" 32 #include "vertex_line2d.h" 33 #include "g2o/core/base_binary_edge.h" 34 #include "g2o/stuff/misc.h" 35 #include "g2o_types_slam2d_addons_api.h" 36 37 namespace g2o { 38 39 class EdgeLine2DPointXY : public BaseBinaryEdge<1, number_t, VertexLine2D, VertexPointXY> //Avoid redefinition of BaseEdge in MSVC 40 { 41 public: 42 G2O_TYPES_SLAM2D_ADDONS_API EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 G2O_TYPES_SLAM2D_ADDONS_API EdgeLine2DPointXY(); 44 computeError()45 G2O_TYPES_SLAM2D_ADDONS_API void computeError() 46 { 47 const VertexLine2D* l = static_cast<const VertexLine2D*>(_vertices[0]); 48 const VertexPointXY* p = static_cast<const VertexPointXY*>(_vertices[1]); 49 Vector2 n(std::cos(l->theta()), std::sin(l->theta())); 50 number_t prediction=n.dot(p->estimate())-l->rho(); 51 _error[0] = prediction - _measurement; 52 } 53 setMeasurementData(const number_t * d)54 G2O_TYPES_SLAM2D_ADDONS_API virtual bool setMeasurementData(const number_t* d){ 55 _measurement = *d; 56 return true; 57 } 58 getMeasurementData(number_t * d)59 G2O_TYPES_SLAM2D_ADDONS_API virtual bool getMeasurementData(number_t* d) const{ 60 *d = _measurement; 61 return true; 62 } 63 measurementDimension()64 G2O_TYPES_SLAM2D_ADDONS_API virtual int measurementDimension() const {return 1;} 65 setMeasurementFromState()66 G2O_TYPES_SLAM2D_ADDONS_API virtual bool setMeasurementFromState(){ 67 const VertexLine2D* l = static_cast<const VertexLine2D*>(_vertices[0]); 68 const VertexPointXY* p = static_cast<const VertexPointXY*>(_vertices[1]); 69 Vector2 n(std::cos(l->theta()), std::sin(l->theta())); 70 number_t prediction=n.dot(p->estimate())-l->rho(); 71 _measurement = prediction; 72 return true; 73 } 74 75 G2O_TYPES_SLAM2D_ADDONS_API virtual bool read(std::istream& is); 76 G2O_TYPES_SLAM2D_ADDONS_API virtual bool write(std::ostream& os) const; 77 78 /* virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); */ 79 /* virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) to; return (from.count(_vertices[0]) == 1 ? 1.0 : -1.0);} */ 80 /* #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES */ 81 /* virtual void linearizeOplus(); */ 82 /* #endif */ 83 }; 84 85 /* class G2O_TYPES_SLAM2D_ADDONS_API EdgeLine2DPointXYWriteGnuplotAction: public WriteGnuplotAction { */ 86 /* public: */ 87 /* EdgeLine2DPointXYWriteGnuplotAction(); */ 88 /* virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, */ 89 /* HyperGraphElementAction::Parameters* params_); */ 90 /* }; */ 91 92 /* #ifdef G2O_HAVE_OPENGL */ 93 /* class G2O_TYPES_SLAM2D_ADDONS_API EdgeLine2DPointXYDrawAction: public DrawAction{ */ 94 /* public: */ 95 /* EdgeLine2DPointXYDrawAction(); */ 96 /* virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, */ 97 /* HyperGraphElementAction::Parameters* params_); */ 98 /* }; */ 99 /* #endif */ 100 101 } // end namespace 102 103 #endif 104