graph_optimizer2d_aux.h
Go to the documentation of this file.
```00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // HOG-Man 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 // HOG-Man 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 #ifndef _GRAPH_OPTIMIZER2D_AUX_HH_
00018 #define _GRAPH_OPTIMIZER2D_AUX_HH_
00019
00020 #include "hogman_minimal/math/transformation.h"
00021 #include <set>
00022 #include <iostream>
00023
00026     int id1, id2;
00027     Transformation2 mean;
00028     Matrix3 informationMatrix;
00029   };
00030
00033       int i11=e1.id1, i12=e1.id2;
00034       if (i11>i12){
00035         i11=e1.id2;
00036       i12=e1.id1;
00037       }
00038       int i21=e2.id1, i22=e2.id2;
00039       if (i21>i22){
00040         i21=e2.id2;
00041         i22=e2.id1;
00042       }
00043       if (i12<i22)
00044         return true;
00045       if (i12>i22)
00046         return false;
00047       return (i11<i21);
00048     }
00049   };
00050