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_pointxy.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 EdgeSE2PointXY::EdgeSE2PointXY() :
00034 BaseBinaryEdge<2, Vector2d, VertexSE2, VertexPointXY>()
00035 {
00036 }
00037
00038 bool EdgeSE2PointXY::read(std::istream& is)
00039 {
00040 is >> measurement()[0] >> measurement()[1];
00041 inverseMeasurement() = measurement() * -1;
00042 is >> information()(0,0) >> information()(0,1) >> information()(1,1);
00043 information()(1,0) = information()(0,1);
00044 return true;
00045 }
00046
00047 bool EdgeSE2PointXY::write(std::ostream& os) const
00048 {
00049 os << measurement()[0] << " " << measurement()[1] << " ";
00050 os << information()(0,0) << " " << information()(0,1) << " " << information()(1,1);
00051 return os.good();
00052 }
00053
00054 void EdgeSE2PointXY::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to)
00055 {
00056 assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXY");
00057
00058 VertexSE2* vi = static_cast<VertexSE2*>(_vertices[0]);
00059 VertexPointXY* vj = static_cast<VertexPointXY*>(_vertices[1]);
00060 if (from.count(vi) > 0 && to == vj) {
00061 vj->estimate() = vi->estimate() * _measurement;
00062 }
00063 }
00064
00065 #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
00066 void EdgeSE2PointXY::linearizeOplus()
00067 {
00068 const VertexSE2* vi = static_cast<const VertexSE2*>(_vertices[0]);
00069 const VertexPointXY* vj = static_cast<const VertexPointXY*>(_vertices[1]);
00070 const double& x1 = vi->estimate().translation()[0];
00071 const double& y1 = vi->estimate().translation()[1];
00072 const double& th1 = vi->estimate().rotation().angle();
00073 const double& x2 = vj->estimate()[0];
00074 const double& y2 = vj->estimate()[1];
00075
00076 double aux_1 = cos(th1) ;
00077 double aux_2 = -aux_1 ;
00078 double aux_3 = sin(th1) ;
00079
00080 _jacobianOplusXi( 0 , 0 ) = aux_2 ;
00081 _jacobianOplusXi( 0 , 1 ) = -aux_3 ;
00082 _jacobianOplusXi( 0 , 2 ) = aux_1*y2-aux_1*y1-aux_3*x2+aux_3*x1 ;
00083 _jacobianOplusXi( 1 , 0 ) = aux_3 ;
00084 _jacobianOplusXi( 1 , 1 ) = aux_2 ;
00085 _jacobianOplusXi( 1 , 2 ) = -aux_3*y2+aux_3*y1-aux_1*x2+aux_1*x1 ;
00086
00087 _jacobianOplusXj( 0 , 0 ) = aux_1 ;
00088 _jacobianOplusXj( 0 , 1 ) = aux_3 ;
00089 _jacobianOplusXj( 1 , 0 ) = -aux_3 ;
00090 _jacobianOplusXj( 1 , 1 ) = aux_1 ;
00091 }
00092 #endif
00093
00094 EdgeSE2PointXYWriteGnuplotAction::EdgeSE2PointXYWriteGnuplotAction(): WriteGnuplotAction(typeid(EdgeSE2PointXY).name()){}
00095
00096 HyperGraphElementAction* EdgeSE2PointXYWriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
00097 if (typeid(*element).name()!=_typeName)
00098 return 0;
00099 WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
00100 if (!params->os){
00101 std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl;
00102 return 0;
00103 }
00104
00105 EdgeSE2PointXY* e = static_cast<EdgeSE2PointXY*>(element);
00106 VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertices()[0]);
00107 VertexPointXY* toEdge = static_cast<VertexPointXY*>(e->vertices()[1]);
00108 *(params->os) << fromEdge->estimate().translation().x() << " " << fromEdge->estimate().translation().y()
00109 << " " << fromEdge->estimate().rotation().angle() << std::endl;
00110 *(params->os) << toEdge->estimate().x() << " " << toEdge->estimate().y() << std::endl;
00111 *(params->os) << std::endl;
00112 return this;
00113 }
00114
00115 #ifdef G2O_HAVE_OPENGL
00116 EdgeSE2PointXYDrawAction::EdgeSE2PointXYDrawAction(): DrawAction(typeid(EdgeSE2PointXY).name()){}
00117
00118 HyperGraphElementAction* EdgeSE2PointXYDrawAction::operator()(HyperGraph::HyperGraphElement* element,
00119 HyperGraphElementAction::Parameters* ){
00120 if (typeid(*element).name()!=_typeName)
00121 return 0;
00122 EdgeSE2PointXY* e = static_cast<EdgeSE2PointXY*>(element);
00123 VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertices()[0]);
00124 VertexPointXY* toEdge = static_cast<VertexPointXY*>(e->vertices()[1]);
00125 glColor3f(0.4,0.4,0.2);
00126 glPushAttrib(GL_ENABLE_BIT);
00127 glDisable(GL_LIGHTING);
00128 glBegin(GL_LINES);
00129 glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
00130 glVertex3f(toEdge->estimate().x(),toEdge->estimate().y(),0.);
00131 glEnd();
00132 glPopAttrib();
00133 return this;
00134 }
00135 #endif
00136
00137 }