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 #include "vertex_se3.h" 28 #include "g2o/core/factory.h" 29 #ifdef G2O_HAVE_OPENGL 30 #include "g2o/stuff/opengl_wrapper.h" 31 #include "g2o/stuff/opengl_primitives.h" 32 #endif 33 34 #include <iostream> 35 #include "g2o/core/cache.h" 36 37 using namespace Eigen; 38 39 namespace g2o { 40 VertexSE3()41 VertexSE3::VertexSE3() : 42 BaseVertex<6, Isometry3>(), 43 _numOplusCalls(0) 44 { 45 setToOriginImpl(); 46 updateCache(); 47 } 48 read(std::istream & is)49 bool VertexSE3::read(std::istream& is) 50 { 51 Vector7 est; 52 bool state = internal::readVector(is, est); 53 setEstimate(internal::fromVectorQT(est)); 54 return state; 55 } 56 write(std::ostream & os) const57 bool VertexSE3::write(std::ostream& os) const 58 { 59 return internal::writeVector(os, internal::toVectorQT(estimate())); 60 } 61 VertexSE3WriteGnuplotAction()62 VertexSE3WriteGnuplotAction::VertexSE3WriteGnuplotAction(): WriteGnuplotAction(typeid(VertexSE3).name()){} 63 operator ()(HyperGraph::HyperGraphElement * element,HyperGraphElementAction::Parameters * params_)64 HyperGraphElementAction* VertexSE3WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ 65 if (typeid(*element).name()!=_typeName) 66 return nullptr; 67 WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_); 68 if (!params->os){ 69 std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid os specified" << std::endl; 70 return nullptr; 71 } 72 73 VertexSE3* v = static_cast<VertexSE3*>(element); 74 Vector6 est=internal::toVectorMQT(v->estimate()); 75 for (int i=0; i<6; i++) 76 *(params->os) << est[i] << " "; 77 *(params->os) << std::endl; 78 return this; 79 } 80 81 #ifdef G2O_HAVE_OPENGL drawTriangle(float xSize,float ySize)82 void drawTriangle(float xSize, float ySize){ 83 Vector3F p[3]; 84 glBegin(GL_TRIANGLES); 85 p[0] << 0., 0., 0.; 86 p[1] << -xSize, ySize, 0.; 87 p[2] << -xSize, -ySize, 0.; 88 for (int i = 1; i < 2; ++i) { 89 Vector3F normal = (p[i] - p[0]).cross(p[i+1] - p[0]); 90 glNormal3f(normal.x(), normal.y(), normal.z()); 91 glVertex3f(p[0].x(), p[0].y(), p[0].z()); 92 glVertex3f(p[i].x(), p[i].y(), p[i].z()); 93 glVertex3f(p[i+1].x(), p[i+1].y(), p[i+1].z()); 94 } 95 glEnd(); 96 } 97 VertexSE3DrawAction()98 VertexSE3DrawAction::VertexSE3DrawAction() 99 : DrawAction(typeid(VertexSE3).name()), _triangleX(nullptr), _triangleY(nullptr) { 100 _cacheDrawActions = 0; 101 } 102 refreshPropertyPtrs(HyperGraphElementAction::Parameters * params_)103 bool VertexSE3DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){ 104 if (!DrawAction::refreshPropertyPtrs(params_)) 105 return false; 106 if (_previousParams){ 107 _triangleX = _previousParams->makeProperty<FloatProperty>(_typeName + "::TRIANGLE_X", .2f); 108 _triangleY = _previousParams->makeProperty<FloatProperty>(_typeName + "::TRIANGLE_Y", .05f); 109 } else { 110 _triangleX = 0; 111 _triangleY = 0; 112 } 113 return true; 114 } 115 operator ()(HyperGraph::HyperGraphElement * element,HyperGraphElementAction::Parameters * params_)116 HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, 117 HyperGraphElementAction::Parameters* params_){ 118 if (typeid(*element).name()!=_typeName) 119 return nullptr; 120 initializeDrawActionsCache(); 121 refreshPropertyPtrs(params_); 122 123 if (! _previousParams) 124 return this; 125 126 if (_show && !_show->value()) 127 return this; 128 129 VertexSE3* that = static_cast<VertexSE3*>(element); 130 131 glColor3f(POSE_VERTEX_COLOR); 132 glPushMatrix(); 133 glMultMatrixd(that->estimate().matrix().cast<double>().eval().data()); 134 opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f); 135 drawCache(that->cacheContainer(), params_); 136 drawUserData(that->userData(), params_); 137 glPopMatrix(); 138 return this; 139 } 140 #endif 141 142 } 143