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_VERTEX_LINE3D_H_ 28 #define G2O_VERTEX_LINE3D_H_ 29 30 #include "g2o/config.h" 31 #include "g2o/core/base_vertex.h" 32 #include "g2o/core/hyper_graph_action.h" 33 34 #include "g2o_types_slam3d_addons_api.h" 35 #include "line3d.h" 36 37 namespace g2o { 38 39 class G2O_TYPES_SLAM3D_ADDONS_API VertexLine3D : public BaseVertex<4, Line3D> { 40 public: 41 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 42 43 VertexLine3D(); 44 virtual bool read(std::istream& is); 45 virtual bool write(std::ostream& os) const; 46 setToOriginImpl()47 virtual void setToOriginImpl() { _estimate = Line3D(); } 48 oplusImpl(const number_t * update_)49 virtual void oplusImpl(const number_t* update_) { 50 Eigen::Map<const Vector4> update(update_); 51 _estimate.oplus(update); 52 } 53 setEstimateDataImpl(const number_t * est)54 virtual bool setEstimateDataImpl(const number_t* est) { 55 Eigen::Map<const Vector6> _est(est); 56 _estimate = Line3D(_est); 57 return true; 58 } 59 getEstimateData(number_t * est)60 virtual bool getEstimateData(number_t* est) const { 61 Eigen::Map<Vector6> _est(est); 62 _est = _estimate; 63 return true; 64 } 65 estimateDimension()66 virtual int estimateDimension() const { 67 return 6; 68 } 69 70 Vector3 color; 71 }; 72 73 #ifdef G2O_HAVE_OPENGL 74 class VertexLine3DDrawAction : public DrawAction { 75 public: 76 VertexLine3DDrawAction(); 77 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 78 HyperGraphElementAction::Parameters* params_); 79 protected: 80 virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_); 81 FloatProperty* _lineLength, *_lineWidth; 82 83 }; 84 #endif 85 86 } 87 #endif 88