Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "edge_se2.h"
00018
00019 #ifdef WINDOWS
00020 #include <windows.h>
00021 #endif
00022
00023 #ifdef G2O_HAVE_OPENGL
00024 #ifdef __APPLE__
00025 #include <OpenGL/gl.h>
00026 #else
00027 #include <GL/gl.h>
00028 #endif
00029 #endif
00030
00031 namespace g2o {
00032
00033 EdgeSE2::EdgeSE2() :
00034 BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2>()
00035 {
00036 }
00037
00038 bool EdgeSE2::read(std::istream& is)
00039 {
00040 Vector3d p;
00041 is >> p[0] >> p[1] >> p[2];
00042 measurement().fromVector(p);
00043 inverseMeasurement() = measurement().inverse();
00044 for (int i = 0; i < 3; ++i)
00045 for (int j = i; j < 3; ++j) {
00046 is >> information()(i, j);
00047 if (i != j)
00048 information()(j, i) = information()(i, j);
00049 }
00050 return true;
00051 }
00052
00053 bool EdgeSE2::write(std::ostream& os) const
00054 {
00055 Vector3d p = measurement().toVector();
00056 os << p.x() << " " << p.y() << " " << p.z();
00057 for (int i = 0; i < 3; ++i)
00058 for (int j = i; j < 3; ++j)
00059 os << " " << information()(i, j);
00060 return os.good();
00061 }
00062
00063 void EdgeSE2::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* )
00064 {
00065 VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
00066 VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);
00067 if (from.count(fromEdge) > 0)
00068 toEdge->estimate() = fromEdge->estimate() * _measurement;
00069 else
00070 fromEdge->estimate() = toEdge->estimate() * _inverseMeasurement;
00071 }
00072
00073 #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
00074 void EdgeSE2::linearizeOplus()
00075 {
00076 const VertexSE2* vi = static_cast<const VertexSE2*>(_vertices[0]);
00077 const VertexSE2* vj = static_cast<const VertexSE2*>(_vertices[1]);
00078 double thetai = vi->estimate().rotation().angle();
00079
00080 Vector2d dt = vj->estimate().translation() - vi->estimate().translation();
00081 double si=sin(thetai), ci=cos(thetai);
00082
00083 _jacobianOplusXi(0, 0) = -ci; _jacobianOplusXi(0, 1) = -si; _jacobianOplusXi(0, 2) = -si*dt.x()+ci*dt.y();
00084 _jacobianOplusXi(1, 0) = si; _jacobianOplusXi(1, 1) = -ci; _jacobianOplusXi(1, 2) = -ci*dt.x()-si*dt.y();
00085 _jacobianOplusXi(2, 0) = 0; _jacobianOplusXi(2, 1) = 0; _jacobianOplusXi(2, 2) = -1;
00086
00087 _jacobianOplusXj(0, 0) = ci; _jacobianOplusXj(0, 1)= si; _jacobianOplusXj(0, 2)= 0;
00088 _jacobianOplusXj(1, 0) =-si; _jacobianOplusXj(1, 1)= ci; _jacobianOplusXj(1, 2)= 0;
00089 _jacobianOplusXj(2, 0) = 0; _jacobianOplusXj(2, 1)= 0; _jacobianOplusXj(2, 2)= 1;
00090
00091 const SE2& rmean = inverseMeasurement();
00092 Matrix3d z = Matrix3d::Zero();
00093 z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
00094 z(2, 2) = 1.;
00095 _jacobianOplusXi = z * _jacobianOplusXi;
00096 _jacobianOplusXj = z * _jacobianOplusXj;
00097 }
00098 #endif
00099
00100 EdgeSE2WriteGnuplotAction::EdgeSE2WriteGnuplotAction(): WriteGnuplotAction(typeid(EdgeSE2).name()){}
00101
00102 HyperGraphElementAction* EdgeSE2WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
00103 if (typeid(*element).name()!=_typeName)
00104 return 0;
00105 WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
00106 if (!params->os){
00107 std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl;
00108 return 0;
00109 }
00110
00111 EdgeSE2* e = static_cast<EdgeSE2*>(element);
00112 VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertices()[0]);
00113 VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertices()[1]);
00114 *(params->os) << fromEdge->estimate().translation().x() << " " << fromEdge->estimate().translation().y()
00115 << " " << fromEdge->estimate().rotation().angle() << std::endl;
00116 *(params->os) << toEdge->estimate().translation().x() << " " << toEdge->estimate().translation().y()
00117 << " " << toEdge->estimate().rotation().angle() << std::endl;
00118 *(params->os) << std::endl;
00119 return this;
00120 }
00121
00122 #ifdef G2O_HAVE_OPENGL
00123 EdgeSE2DrawAction::EdgeSE2DrawAction(): DrawAction(typeid(EdgeSE2).name()){}
00124
00125 HyperGraphElementAction* EdgeSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
00126 HyperGraphElementAction::Parameters* ){
00127 if (typeid(*element).name()!=_typeName)
00128 return 0;
00129 EdgeSE2* e = static_cast<EdgeSE2*>(element);
00130 VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertices()[0]);
00131 VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertices()[1]);
00132 glColor3f(0.5,0.5,0.8);
00133 glPushAttrib(GL_ENABLE_BIT);
00134 glDisable(GL_LIGHTING);
00135 glBegin(GL_LINES);
00136 glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
00137 glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
00138 glEnd();
00139 glPopAttrib();
00140 return this;
00141 }
00142 #endif
00143
00144 }