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_SEGMENT2D_LINE_H
28 #define G2O_EDGE_SE2_SEGMENT2D_LINE_H
29 
30 #include "g2o/config.h"
31 #include "g2o/core/base_binary_edge.h"
32 #include "g2o/types/slam2d/vertex_se2.h"
33 #include "g2o_types_slam2d_addons_api.h"
34 #include "vertex_segment2d.h"
35 
36 namespace g2o {
37 
38 class EdgeSE2Segment2DLine
39     : public BaseBinaryEdge<2, Vector2, VertexSE2, VertexSegment2D>  // 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 EdgeSE2Segment2DLine();
44 
theta()45   G2O_TYPES_SLAM2D_ADDONS_API number_t theta() const { return _measurement[0]; }
rho()46   G2O_TYPES_SLAM2D_ADDONS_API number_t rho() const { return _measurement[1]; }
47 
setTheta(number_t t)48   G2O_TYPES_SLAM2D_ADDONS_API void setTheta(number_t t) { _measurement[0] = t; }
setRho(number_t r)49   G2O_TYPES_SLAM2D_ADDONS_API void setRho(number_t r) { _measurement[1] = r; }
50 
computeError()51   G2O_TYPES_SLAM2D_ADDONS_API void computeError() {
52     const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]);
53     const VertexSegment2D* l2 = static_cast<const VertexSegment2D*>(_vertices[1]);
54     SE2 iEst = v1->estimate().inverse();
55     Vector2 predP1 = iEst * l2->estimateP1();
56     Vector2 predP2 = iEst * l2->estimateP2();
57     Vector2 dP = predP2 - predP1;
58     Vector2 normal(dP.y(), -dP.x());
59     normal.normalize();
60     Vector2 prediction(std::atan2(normal.y(), normal.x()), predP1.dot(normal) * .5 + predP2.dot(normal) * .5);
61 
62     _error = prediction - _measurement;
63     _error[0] = normalize_theta(_error[0]);
64   }
65 
setMeasurementData(const number_t * d)66   G2O_TYPES_SLAM2D_ADDONS_API virtual bool setMeasurementData(const number_t* d) {
67     Eigen::Map<const Vector2> data(d);
68     _measurement = data;
69     return true;
70   }
71 
getMeasurementData(number_t * d)72   G2O_TYPES_SLAM2D_ADDONS_API virtual bool getMeasurementData(number_t* d) const {
73     Eigen::Map<Vector2> data(d);
74     data = _measurement;
75     return true;
76   }
77 
measurementDimension()78   G2O_TYPES_SLAM2D_ADDONS_API virtual int measurementDimension() const { return 2; }
79 
setMeasurementFromState()80   G2O_TYPES_SLAM2D_ADDONS_API virtual bool setMeasurementFromState() {
81     const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]);
82     const VertexSegment2D* l2 = static_cast<const VertexSegment2D*>(_vertices[1]);
83     SE2 iEst = v1->estimate().inverse();
84     Vector2 predP1 = iEst * l2->estimateP1();
85     Vector2 predP2 = iEst * l2->estimateP2();
86     Vector2 dP = predP2 - predP1;
87     Vector2 normal(dP.y(), -dP.x());
88     normal.normalize();
89     Vector2 prediction(std::atan2(normal.y(), normal.x()), predP1.dot(normal) * .5 + predP2.dot(normal) * .5);
90     _measurement = prediction;
91     return true;
92   }
93 
94   G2O_TYPES_SLAM2D_ADDONS_API virtual bool read(std::istream& is);
95   G2O_TYPES_SLAM2D_ADDONS_API virtual bool write(std::ostream& os) const;
96 
97   /* #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES */
98   /*       virtual void linearizeOplus(); */
99   /* #endif */
100 };
101 
102 /*   class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DLineWriteGnuplotAction: public WriteGnuplotAction { */
103 /*   public: */
104 /*     EdgeSE2Segment2DLineWriteGnuplotAction(); */
105 /*     virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element,  */
106 /*             HyperGraphElementAction::Parameters* params_); */
107 /*   }; */
108 
109 /* #ifdef G2O_HAVE_OPENGL */
110 /*   class G2O_TYPES_SLAM2D_ADDONS_API EdgeSE2Segment2DLineDrawAction: public DrawAction{ */
111 /*   public: */
112 /*     EdgeSE2Segment2DLineDrawAction(); */
113 /*     virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element,  */
114 /*             HyperGraphElementAction::Parameters* params_); */
115 /*   }; */
116 /* #endif */
117 
118 }  // namespace g2o
119 
120 #endif
121