edge_se3_quat.cpp
Go to the documentation of this file.
00001 // g2o - General Graph Optimization
00002 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
00003 // 
00004 // g2o is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // g2o is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include "edge_se3_quat.h"
00018 #include "g2o/core/factory.h"
00019 
00020 #ifdef G2O_HAVE_OPENGL
00021 #ifdef __APPLE__
00022 #include <OpenGL/gl.h>
00023 #else
00024 #include <GL/gl.h>
00025 #endif
00026 #endif
00027 
00028 #include <iostream>
00029 
00030 namespace g2o {
00031 
00032   // forward declaration for the analytic jacobian
00033   void  jacobian_3d_qman ( Matrix< double, 6 , 6> &  Ji , Matrix< double, 6 , 6> &  Jj, const double&  z11 , const double&  z12 , const double&  z13 , const double&  z14 , const double&  z21 , const double&  z22 , const double&  z23 , const double&  z24 , const double&  z31 , const double&  z32 , const double&  z33 , const double&  z34 , const double&  xab11 , const double&  xab12 , const double&  xab13 , const double&  xab14 , const double&  xab21 , const double&  xab22 , const double&  xab23 , const double&  xab24 , const double&  xab31 , const double&  xab32 , const double&  xab33 , const double&  xab34 );
00034 
00035   using namespace std;
00036 
00037 
00038   EdgeSE3::EdgeSE3() :
00039     BaseBinaryEdge<6, SE3Quat, VertexSE3, VertexSE3>()
00040   {
00041   }
00042 
00043 
00044   bool EdgeSE3::setMeasurementFromState(){
00045     const VertexSE3* v1 = dynamic_cast<const VertexSE3*>(_vertices[0]);
00046     const VertexSE3* v2 = dynamic_cast<const VertexSE3*>(_vertices[1]);
00047     _measurement = (v1->estimate().inverse()*v2->estimate());
00048     _inverseMeasurement = _measurement.inverse();
00049     return true;
00050   }
00051 
00052   bool EdgeSE3::read(std::istream& is)
00053   {
00054     for (int i=0; i<7; i++)
00055       is >> measurement()[i];
00056     measurement().rotation().normalize();
00057     inverseMeasurement() = measurement().inverse();
00058     
00059     for (int i=0; i<6; i++)
00060       for (int j=i; j<6; j++) {
00061         is >> information()(i,j);
00062         if (i!=j)
00063           information()(j,i) = information()(i,j);
00064       }
00065     return true;
00066   }
00067 
00068   bool EdgeSE3::write(std::ostream& os) const
00069   {
00070     for (int i=0; i<7; i++)
00071       os << measurement()[i] << " ";
00072     for (int i=0; i<6; i++)
00073       for (int j=i; j<6; j++){
00074         os << " " <<  information()(i,j);
00075       }
00076     return os.good();
00077   }
00078 
00079   void EdgeSE3::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/)
00080   {
00081     VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
00082     VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]);
00083     if (from_.count(from) > 0)
00084       to->estimate() = from->estimate() * _measurement;
00085     else
00086       from->estimate() = to->estimate() * _inverseMeasurement;
00087   }
00088 
00089 #ifdef EDGE_SE3_QUAT_ANALYTIC_JACOBIAN
00090   void EdgeSE3::linearizeOplus(){
00091     VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
00092     VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]);
00093     
00094     Matrix3d izR        = inverseMeasurement().rotation().toRotationMatrix();
00095     const Vector3d& izt = inverseMeasurement().translation();
00096     
00097     SE3Quat iXiXj         = from->estimate().inverse() * to->estimate();
00098     Matrix3d iRiRj        = iXiXj.rotation().toRotationMatrix();
00099     const Vector3d& ititj = iXiXj.translation();
00100 
00101     jacobian_3d_qman ( _jacobianOplusXi , _jacobianOplusXj,
00102                        izR(0,0), izR(0,1), izR(0,2), izt(0),
00103                        izR(1,0), izR(1,1), izR(1,2), izt(1),
00104                        izR(2,0), izR(2,1), izR(2,2), izt(2),
00105                        iRiRj(0,0), iRiRj(0,1), iRiRj(0,2), ititj(0),
00106                        iRiRj(1,0), iRiRj(1,1), iRiRj(1,2), ititj(1),
00107                        iRiRj(2,0), iRiRj(2,1), iRiRj(2,2), ititj(2));
00108   }
00109 #endif
00110 
00111 
00112 
00113   EdgeSE3WriteGnuplotAction::EdgeSE3WriteGnuplotAction(): WriteGnuplotAction(typeid(EdgeSE3).name()){}
00114 
00115   HyperGraphElementAction* EdgeSE3WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
00116     if (typeid(*element).name()!=_typeName)
00117       return 0;
00118     WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
00119     if (!params->os){
00120       std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl;
00121       return 0;
00122     }
00123 
00124     EdgeSE3* e =  static_cast<EdgeSE3*>(element);
00125     VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
00126     VertexSE3* toEdge   = static_cast<VertexSE3*>(e->vertices()[1]);
00127     *(params->os) << fromEdge->estimate().translation().x() << " " 
00128                   << fromEdge->estimate().translation().y() << " " 
00129                   << fromEdge->estimate().translation().z() << " ";
00130     *(params->os) << fromEdge->estimate().rotation().x() << " " 
00131                   << fromEdge->estimate().rotation().y() << " " 
00132                   << fromEdge->estimate().rotation().z() << " " << std::endl;
00133     *(params->os) << toEdge->estimate().translation().x() << " " 
00134                   << toEdge->estimate().translation().y() << " " 
00135                   << toEdge->estimate().translation().z() << " ";
00136     *(params->os) << toEdge->estimate().rotation().x() << " " 
00137                   << toEdge->estimate().rotation().y() << " " 
00138                   << toEdge->estimate().rotation().z() << " " << std::endl;
00139     *(params->os) << std::endl;
00140     return this;
00141   }
00142 
00143 #ifdef G2O_HAVE_OPENGL
00144   EdgeSE3DrawAction::EdgeSE3DrawAction(): DrawAction(typeid(EdgeSE3).name()){}
00145 
00146   HyperGraphElementAction* EdgeSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, 
00147                                                          HyperGraphElementAction::Parameters* /*params_*/){
00148     if (typeid(*element).name()!=_typeName)
00149       return 0;
00150     EdgeSE3* e =  static_cast<EdgeSE3*>(element);
00151     VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
00152     VertexSE3* toEdge   = static_cast<VertexSE3*>(e->vertices()[1]);
00153     glColor3f(0.5,0.5,0.8);
00154     glPushAttrib(GL_ENABLE_BIT);
00155     glDisable(GL_LIGHTING);
00156     glBegin(GL_LINES);
00157     glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z());
00158     glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z());
00159     glEnd();
00160     glPopAttrib();
00161     return this;
00162   }
00163 #endif
00164 
00165 } // end namespace


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:31:02