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_PLANE3D_H 28 #define G2O_EDGE_PLANE3D_H 29 30 #include "g2o/config.h" 31 #include "g2o/core/base_binary_edge.h" 32 #include "g2o_types_slam3d_addons_api.h" 33 #include "vertex_plane.h" 34 35 namespace g2o { 36 37 class G2O_TYPES_SLAM3D_ADDONS_API EdgePlane : public BaseBinaryEdge<4, Vector4, VertexPlane, VertexPlane> { 38 public: 39 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 EdgePlane(); 41 computeError()42 void computeError() { 43 const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]); 44 const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]); 45 _error = (v2->estimate().toVector() - v1->estimate().toVector()) - _measurement; 46 } 47 virtual bool read(std::istream& is); 48 virtual bool write(std::ostream& os) const; 49 setMeasurement(const Vector4 & m)50 virtual void setMeasurement(const Vector4& m) { _measurement = m; } 51 setMeasurementData(const number_t * d)52 virtual bool setMeasurementData(const number_t* d) { 53 Eigen::Map<const Vector4> m(d); 54 _measurement = m; 55 return true; 56 } 57 getMeasurementData(number_t * d)58 virtual bool getMeasurementData(number_t* d) const { 59 Eigen::Map<Vector4> m(d); 60 m = _measurement; 61 return true; 62 } 63 measurementDimension()64 virtual int measurementDimension() const { return 4; } 65 setMeasurementFromState()66 virtual bool setMeasurementFromState() { 67 const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]); 68 const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]); 69 _measurement = (v2->estimate().toVector()) - v1->estimate().toVector(); 70 71 return true; 72 } 73 74 #if 0 75 virtual void linearizeOplus(); 76 #endif 77 }; 78 79 } // namespace g2o 80 81 #endif 82