root / rgbdslam / src / edge.h @ 9240aaa3
History  View  Annotate  Download (1.46 KB)
1 
// HOGMan  Hierarchical Optimization for Pose Graphs on Manifolds


2 
// Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss

3 
//

4 
// HOGMan 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 
// HOGMan 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
