Statistics
| Branch: | Revision:

root / rgbdslam / src / edge.h @ 9240aaa3

History | View | Annotate | Download (1.46 KB)

1
// HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
2
// Copyright (C) 2010 G. Grisetti, R. K├╝mmerle, C. Stachniss
3
// 
4
// HOG-Man is free software: you can redistribute it and/or modify
5
// it under the terms of the GNU Lesser General Public License as published
6
// by the Free Software Foundation, either version 3 of the License, or
7
// (at your option) any later version.
8
// 
9
// HOG-Man is distributed in the hope that it will be useful,
10
// but WITHOUT ANY WARRANTY; without even the implied warranty of
11
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12
// GNU Lesser General Public License for more details.
13
// 
14
// You should have received a copy of the GNU Lesser General Public License
15
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
16

    
17
#ifndef RGBDSLAMEDGES_3D_H
18
#define RGBDSLAMEDGES_3D_H
19

    
20
#include <set>
21
#include <iostream>
22
#include "g2o/math_groups/se3quat.h"
23

    
24
struct LoadedEdge3D
25
{
26
  int id1, id2;
27
  g2o::SE3Quat mean;
28
  Eigen::Matrix<double, 6,6> informationMatrix;
29
};
30

    
31
struct LoadedEdgeComparator3D
32
{
33
  inline bool operator()(const LoadedEdge3D& e1, const LoadedEdge3D& e2){
34
    int i11=e1.id1, i12=e1.id2;
35
    if (i11>i12){
36
      i11=e1.id2;
37
      i12=e1.id1;
38
    }
39
    int i21=e2.id1, i22=e2.id2;
40
    if (i21>i22){
41
      i21=e2.id2;
42
      i22=e2.id1;
43
    }
44
    if (i12<i22) return true;
45
    if (i12>i22) return false;
46
    return (i11<i21);
47
  }
48
};
49

    
50
typedef std::set<LoadedEdge3D, LoadedEdgeComparator3D> LoadedEdgeSet3D;
51

    
52
#endif