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
|