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_SE2_POINT_XY_OFFSET_H_ 28 #define G2O_EDGE_SE2_POINT_XY_OFFSET_H_ 29 30 #include "g2o/core/base_binary_edge.h" 31 32 #include "vertex_se2.h" 33 #include "vertex_point_xy.h" 34 #include "g2o_types_slam2d_api.h" 35 36 namespace g2o { 37 38 39 /*! \class EdgeSE2PointXYOffset 40 * \brief g2o edge from a track to a point node 41 */ 42 // first two args are the measurement type, second two the connection classes 43 class G2O_TYPES_SLAM2D_API EdgeSE2PointXYOffset : public BaseBinaryEdge<2, Vector2, VertexSE2, VertexPointXY> { 44 public: 45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 EdgeSE2PointXYOffset(); 47 virtual bool read(std::istream& is); 48 virtual bool write(std::ostream& os) const; 49 50 void computeError(); 51 virtual void linearizeOplus(); 52 53 setMeasurement(const Vector2 & m)54 virtual void setMeasurement(const Vector2& m){ 55 _measurement = m; 56 } 57 setMeasurementData(const number_t * d)58 virtual bool setMeasurementData(const number_t* d){ 59 Eigen::Map<const Vector2> v(d); 60 _measurement = v; 61 return true; 62 } 63 getMeasurementData(number_t * d)64 virtual bool getMeasurementData(number_t* d) const{ 65 Eigen::Map<Vector2> v(d); 66 v=_measurement; 67 return true; 68 } 69 measurementDimension()70 virtual int measurementDimension() const {return 3;} 71 72 virtual bool setMeasurementFromState() ; 73 initialEstimatePossible(const OptimizableGraph::VertexSet & from,OptimizableGraph::Vertex * to)74 virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { 75 (void)to; 76 return (from.count(_vertices[0]) == 1 ? 1.0 : -1.0); 77 } 78 79 virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 80 81 private: 82 ParameterSE2Offset* offsetParam; 83 CacheSE2Offset* cache; 84 virtual bool resolveCaches(); 85 86 }; 87 88 } 89 #endif 90