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 LOADEDGES_3D_H 00018 #define LOADEDGES_3D_H 00019 00020 #include <set> 00021 #include <iostream> 00022 00023 #include "hogman_minimal/math/transformation.h" 00024 00025 namespace AISNavigation { 00026 00027 struct LoadedEdge3D 00028 { 00029 int id1, id2; 00030 Transformation3 mean; 00031 Matrix6 informationMatrix; 00032 }; 00033 00034 struct LoadedEdgeComparator3D 00035 { 00036 inline bool operator()(const LoadedEdge3D& e1, const LoadedEdge3D& e2){ 00037 int i11=e1.id1, i12=e1.id2; 00038 if (i11>i12){ 00039 i11=e1.id2; 00040 i12=e1.id1; 00041 } 00042 int i21=e2.id1, i22=e2.id2; 00043 if (i21>i22){ 00044 i21=e2.id2; 00045 i22=e2.id1; 00046 } 00047 if (i12<i22) 00048 return true; 00049 if (i12>i22) 00050 return false; 00051 return (i11<i21); 00052 } 00053 }; 00054 00055 typedef std::set<LoadedEdge3D, LoadedEdgeComparator3D> LoadedEdgeSet3D; 00056 00057 void loadEdges3D(LoadedEdgeSet3D& edges, std::istream& is, bool overrideCovariances = false); 00058 00059 }// end namespace 00060 00061 #endif