edge_se2MaxMixture.cpp
Go to the documentation of this file.
00001 /*
00002  * edge_se2MaxMixture.cpp
00003  *
00004  *  Created on: 12.06.2012
00005  *      Author: niko
00006  */
00007 
00008 
00009 #include "edge_se2MaxMixture.h"
00010 
00011 using namespace std;
00012 using namespace Eigen;
00013 
00014 // ================================================
00015 EdgeSE2MaxMixture::EdgeSE2MaxMixture()
00016 {
00017   nullHypothesisMoreLikely = false;
00018 }
00019 
00020 // ================================================
00021 bool EdgeSE2MaxMixture::read(std::istream& is)
00022   {
00023     Vector3d p;
00024     is >>  weight >> p[0] >> p[1] >> p[2];
00025     setMeasurement(g2o::SE2(p));
00026     _inverseMeasurement = measurement().inverse();
00027     //measurement().fromVector(p);
00028     //inverseMeasurement() = measurement().inverse();
00029     for (int i = 0; i < 3; ++i)
00030       for (int j = i; j < 3; ++j) {
00031         is >> information()(i, j);
00032         if (i != j)
00033           information()(j, i) = information()(i, j);
00034       }
00035 
00036     information_constraint = _information;
00037     nu_constraint = 1.0/sqrt(information_constraint.inverse().determinant());
00038     information_nullHypothesis = information_constraint*weight;
00039     nu_nullHypothesis = 1.0/sqrt(information_nullHypothesis.inverse().determinant());
00040 
00041     return true;
00042   }
00043 // ================================================
00044 bool EdgeSE2MaxMixture::write(std::ostream& os) const
00045 {
00046     Vector3d p = measurement().toVector();
00047     os << p.x() << " " << p.y() << " " << p.z();
00048     for (int i = 0; i < 3; ++i)
00049       for (int j = i; j < 3; ++j)
00050         os << " " << information()(i, j);
00051     return os.good();
00052 }
00053 
00054 // ================================================
00055 void EdgeSE2MaxMixture::linearizeOplus()
00056 {
00057   g2o::EdgeSE2::linearizeOplus();
00058   if (nullHypothesisMoreLikely) {
00059     _jacobianOplusXi *= weight;
00060     _jacobianOplusXj *= weight;
00061   }
00062 }
00063 
00064 // ================================================
00065 void EdgeSE2MaxMixture::computeError()
00066 {
00067   // calculate the error for this constraint
00068   g2o::EdgeSE2::computeError();
00069 
00070   // determine the likelihood for constraint and null hypothesis
00071   double mahal_constraint = _error.transpose() * information_constraint * _error;
00072   double likelihood_constraint = nu_constraint * exp(-mahal_constraint);
00073 
00074   double mahal_nullHypothesis  = _error.transpose() * (information_nullHypothesis) * _error;
00075   double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis);
00076 
00077   // if the nullHypothesis is more likely ...
00078   if (likelihood_nullHypothesis > likelihood_constraint) {
00079     _information = information_nullHypothesis;
00080     nullHypothesisMoreLikely = true;
00081   }
00082   else {
00083     _information = information_constraint;
00084     nullHypothesisMoreLikely = false;
00085   }
00086 
00087 
00088 }
00089 
00090 /*
00091 #include <GL/gl.h>
00092 // ================================================
00093 #ifdef G2O_HAVE_OPENGL
00094   EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){}
00095 
00096   g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
00097                g2o::HyperGraphElementAction::Parameters* ){
00098     if (typeid(*element).name()!=_typeName)
00099       return 0;
00100     EdgeSE2MaxMixture* e =  static_cast<EdgeSE2MaxMixture*>(element);
00101 
00102 
00103     g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
00104     g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
00105 
00106 
00107     if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0);
00108     else glColor3f(1.0,0.5,0.2);
00109 
00110     glPushAttrib(GL_ENABLE_BIT);
00111     glDisable(GL_LIGHTING);
00112     glBegin(GL_LINES);
00113     glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
00114     glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
00115     glEnd();
00116     glPopAttrib();
00117     return this;
00118   }
00119 #endif
00120 */
00121 
00122 


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:16