Graph.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef GRAPH_H_
29 #define GRAPH_H_
30 
31 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
32 
33 #include <map>
34 #include <list>
35 #include <rtabmap/core/Link.h>
36 #include <rtabmap/core/GeodeticCoords.h>
37 
38 namespace rtabmap {
39 class Memory;
40 
41 namespace graph {
42 
44 // Graph utilities
46 
47 bool RTABMAP_EXP exportPoses(
48  const std::string & filePath,
49  int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
50  const std::map<int, Transform> & poses,
51  const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
52  const std::map<int, double> & stamps = std::map<int, double>(), // required for format 1
53  bool g2oRobust = false); // optional for format 4
54 
55 bool RTABMAP_EXP importPoses(
56  const std::string & filePath,
57  int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
58  std::map<int, Transform> & poses,
59  std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
60  std::map<int, double> * stamps = 0); // optional for format 1
61 
62 bool RTABMAP_EXP exportGPS(
63  const std::string & filePath,
64  const std::map<int, GPS> & gpsValues,
65  unsigned int rgba = 0xFFFFFFFF);
66 
75 void RTABMAP_EXP calcKittiSequenceErrors(
76  const std::vector<Transform> &poses_gt,
77  const std::vector<Transform> &poses_result,
78  float & t_err,
79  float & r_err);
80 
89 Transform RTABMAP_EXP calcRMSE(
90  const std::map<int, Transform> &groundTruth,
91  const std::map<int, Transform> &poses,
92  float & translational_rmse,
93  float & translational_mean,
94  float & translational_median,
95  float & translational_std,
96  float & translational_min,
97  float & translational_max,
98  float & rotational_rmse,
99  float & rotational_mean,
100  float & rotational_median,
101  float & rotational_std,
102  float & rotational_min,
103  float & rotational_max);
104 
105 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
106  std::multimap<int, Link> & links,
107  int from,
108  int to,
109  bool checkBothWays = true);
110 std::multimap<int, int>::iterator RTABMAP_EXP findLink(
111  std::multimap<int, int> & links,
112  int from,
113  int to,
114  bool checkBothWays = true);
115 std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
116  const std::multimap<int, Link> & links,
117  int from,
118  int to,
119  bool checkBothWays = true);
120 std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
121  const std::multimap<int, int> & links,
122  int from,
123  int to,
124  bool checkBothWays = true);
125 
126 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
127  const std::multimap<int, Link> & links);
128 std::multimap<int, Link> RTABMAP_EXP filterLinks(
129  const std::multimap<int, Link> & links,
130  Link::Type filteredType);
131 std::map<int, Link> RTABMAP_EXP filterLinks(
132  const std::map<int, Link> & links,
133  Link::Type filteredType);
134 
135 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
136 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
137  const std::map<int, Transform> & poses,
138  const Transform & cameraPose,
139  float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
140  float verticalFOV = 45.0f, // in degrees, yfov = atan((image_height/2)/fy)*2
141  float nearClipPlaneDistance = 0.1f,
142  float farClipPlaneDistance = 100.0f,
143  bool negative = false);
144 
153 std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
154  const std::map<int, Transform> & poses,
155  float radius,
156  float angle,
157  bool keepLatest = true);
158 
166 std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
167  const std::map<int, Transform> & poses,
168  float radius,
169  float angle);
170 
171 void reduceGraph(
172  const std::map<int, Transform> & poses,
173  const std::multimap<int, Link> & links,
174  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
175  std::multimap<int, Link> & hyperLinks);
176 
186 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
187  const std::map<int, rtabmap::Transform> & poses,
188  const std::multimap<int, int> & links,
189  int from,
190  int to,
191  bool updateNewCosts = false);
192 
203 std::list<int> RTABMAP_EXP computePath(
204  const std::multimap<int, Link> & links,
205  int from,
206  int to,
207  bool updateNewCosts = false,
208  bool useSameCostForAllLinks = false);
209 
219 std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
220  int fromId,
221  int toId,
222  const Memory * memory,
223  bool lookInDatabase = true,
224  bool updateNewCosts = false,
225  float linearVelocity = 0.0f, // m/sec
226  float angularVelocity = 0.0f); // rad/sec
227 
228 int RTABMAP_EXP findNearestNode(
229  const std::map<int, rtabmap::Transform> & nodes,
230  const rtabmap::Transform & targetPose);
231 
232 std::vector<int> RTABMAP_EXP findNearestNodes(
233  const std::map<int, rtabmap::Transform> & nodes,
234  const rtabmap::Transform & targetPose,
235  int k);
236 
244 std::map<int, float> RTABMAP_EXP getNodesInRadius(
245  int nodeId,
246  const std::map<int, Transform> & nodes,
247  float radius);
248 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
249  int nodeId,
250  const std::map<int, Transform> & nodes,
251  float radius,
252  float angle = 0.0f);
253 
254 float RTABMAP_EXP computePathLength(
255  const std::vector<std::pair<int, Transform> > & path,
256  unsigned int fromIndex = 0,
257  unsigned int toIndex = 0);
258 
259 // assuming they are all linked in map order
260 float RTABMAP_EXP computePathLength(
261  const std::map<int, Transform> & path);
262 
263 std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
264  std::map<int, Transform> poses,
265  const std::multimap<int, Link> & links);
266 
267 
268 } /* namespace graph */
269 
270 } /* namespace rtabmap */
271 #endif /* GRAPH_H_ */
rtabmap::graph::getPosesInRadius
std::map< int, Transform > RTABMAP_EXP getPosesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)
Definition: Graph.cpp:1970
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), bool g2oRobust=false)
Definition: Graph.cpp:53
rtabmap::graph::reduceGraph
void reduceGraph(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, std::multimap< int, int > &hyperNodes, std::multimap< int, Link > &hyperLinks)
Definition: Graph.cpp:1213
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1045
f
f
rtabmap::graph::exportGPS
bool RTABMAP_EXP exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:467
rtabmap::graph::findNearestNodes
std::vector< int > RTABMAP_EXP findNearestNodes(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k)
Definition: Graph.cpp:1883
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::graph::filterDuplicateLinks
std::multimap< int, Link > RTABMAP_EXP filterDuplicateLinks(const std::multimap< int, Link > &links)
Definition: Graph.cpp:952
RTABMAP_EXP
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:684
rtabmap::graph::computePath
std::list< std::pair< int, Transform > > RTABMAP_EXP computePath(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
Definition: Graph.cpp:1450
rtabmap::graph::findNearestNode
int RTABMAP_EXP findNearestNode(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose)
Definition: Graph.cpp:1870
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1157
rtabmap::graph::getNodesInRadius
std::map< int, float > RTABMAP_EXP getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius)
Definition: Graph.cpp:1918
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true)
Definition: Graph.cpp:825
rtabmap::graph::importPoses
bool RTABMAP_EXP importPoses(const std::string &filePath, int format, std::map< int, Transform > &poses, std::multimap< int, Link > *constraints=0, std::map< int, double > *stamps=0)
Definition: Graph.cpp:162
rtabmap::graph::getPaths
std::list< std::map< int, Transform > > RTABMAP_EXP getPaths(std::map< int, Transform > poses, const std::multimap< int, Link > &links)
Definition: Graph.cpp:2088
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2039
rtabmap::graph::filterLinks
std::multimap< int, Link > RTABMAP_EXP filterLinks(const std::multimap< int, Link > &links, Link::Type filteredType)
Definition: Graph.cpp:966
rtabmap::graph::frustumPosesFiltering
std::map< int, Transform > RTABMAP_EXP frustumPosesFiltering(const std::map< int, Transform > &poses, const Transform &cameraPose, float horizontalFOV=45.0f, float verticalFOV=45.0f, float nearClipPlaneDistance=0.1f, float farClipPlaneDistance=100.0f, bool negative=false)
Definition: Graph.cpp:996
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_EXP calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:616

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31