Classes | Typedefs | Functions | Variables
rtabmap::graph Namespace Reference

Classes

struct  errors
 
class  Node
 
struct  Order
 

Typedefs

typedef std::map< int, float > _mapIntFloat
 
typedef std::map< int, Transform_mapIntTransform
 
typedef std::pair< int, float > Pair
 

Functions

void RTABMAP_EXP calcKittiSequenceErrors (const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
 
void RTABMAP_EXP calcRelativeErrors (const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
 
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)
 
void RTABMAP_EXP computeMaxGraphErrors (const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, float &maxLinearErrorRatio, float &maxAngularErrorRatio, float &maxLinearError, float &maxAngularError, const Link **maxLinearErrorLink=0, const Link **maxAngularErrorLink=0, bool for3DoF=false)
 
void RTABMAP_EXP computeMinMax (const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
 
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)
 
std::list< int > RTABMAP_EXP computePath (const std::multimap< int, Link > &links, int from, int to, bool updateNewCosts=false, bool useSameCostForAllLinks=false)
 
std::list< std::pair< int, Transform > > RTABMAP_EXP computePath (int fromId, int toId, const Memory *memory, bool lookInDatabase=true, bool updateNewCosts=false, float linearVelocity=0.0f, float angularVelocity=0.0f)
 
float RTABMAP_EXP computePathLength (const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
 
float RTABMAP_EXP computePathLength (const std::map< int, Transform > &path)
 
bool RTABMAP_EXP exportGPS (const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
 
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 >(), const ParametersMap &parameters=ParametersMap())
 
std::multimap< int, Link > RTABMAP_EXP filterDuplicateLinks (const std::multimap< int, Link > &links)
 
std::multimap< int, Link > RTABMAP_EXP filterLinks (const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
 
std::map< int, Link > RTABMAP_EXP filterLinks (const std::map< int, Link > &links, Link::Type filteredType, bool inverted=false)
 
std::multimap< int, Link >::iterator RTABMAP_EXP findLink (std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
 
std::multimap< int, int >::iterator RTABMAP_EXP findLink (std::multimap< int, int > &links, int from, int to, bool checkBothWays=true)
 
std::multimap< int, Link >::const_iterator RTABMAP_EXP findLink (const std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
 
std::multimap< int, int >::const_iterator RTABMAP_EXP findLink (const std::multimap< int, int > &links, int from, int to, bool checkBothWays=true)
 
std::list< Link > RTABMAP_EXP findLinks (const std::multimap< int, Link > &links, int from)
 
int RTABMAP_EXP findNearestNode (const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
 
std::map< int, float > RTABMAP_EXP findNearestNodes (int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, float > RTABMAP_EXP findNearestNodes (const Transform &targetPose, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, float > findNearestNodes (const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k)
 
std::map< int, Transform > RTABMAP_EXP findNearestPoses (int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, Transform > RTABMAP_EXP findNearestPoses (const Transform &targetPose, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
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)
 
std::vector< double > RTABMAP_EXP getMaxOdomInf (const std::multimap< int, Link > &links)
 
std::map< int, float > getNodesInRadius (int nodeId, const std::map< int, Transform > &nodes, float radius)
 
std::map< int, float > getNodesInRadius (const Transform &targetPose, const std::map< int, Transform > &nodes, float radius)
 
std::list< std::map< int, Transform > > RTABMAP_EXP getPaths (std::map< int, Transform > poses, const std::multimap< int, Link > &links)
 
std::map< int, TransformgetPosesInRadius (int nodeId, const std::map< int, Transform > &nodes, float radius, float angle)
 
std::map< int, TransformgetPosesInRadius (const Transform &targetPose, const std::map< int, Transform > &nodes, float radius, float angle)
 
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)
 
int32_t lastFrameFromSegmentLength (std::vector< float > &dist, int32_t first_frame, float len)
 
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering (const std::map< int, Transform > &poses, float radius, float angle)
 
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering (const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
 
void reduceGraph (const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, std::multimap< int, int > &hyperNodes, std::multimap< int, Link > &hyperLinks)
 
float rotationError (const Transform &pose_error)
 
 RTABMAP_DEPRECATED (_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.")
 
 RTABMAP_DEPRECATED (_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius), "Renamed to findNearestNodes()")
 
 RTABMAP_DEPRECATED (_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform &targetPose, const std::map< int, Transform > &nodes, float radius), "Renamed to findNearestNodes()")
 
 RTABMAP_DEPRECATED (_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f), "Renamed to findNearestNodes()")
 
 RTABMAP_DEPRECATED (_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform &targetPose, const std::map< int, Transform > &nodes, float radius, float angle=0.0f), "Renamed to findNearestNodes()")
 
std::vector< float > trajectoryDistances (const std::vector< Transform > &poses)
 
float translationError (const Transform &pose_error)
 

Variables

float lengths [] = {100,200,300,400,500,600,700,800}
 
int32_t num_lengths = 8
 

Typedef Documentation

◆ _mapIntFloat

typedef std::map<int, float> rtabmap::graph::_mapIntFloat

Definition at line 315 of file Graph.h.

◆ _mapIntTransform

Definition at line 316 of file Graph.h.

◆ Pair

typedef std::pair<int, float> rtabmap::graph::Pair

Definition at line 1648 of file Graph.cpp.

Function Documentation

◆ calcKittiSequenceErrors()

void rtabmap::graph::calcKittiSequenceErrors ( const std::vector< Transform > &  poses_gt,
const std::vector< Transform > &  poses_result,
float &  t_err,
float &  r_err 
)

Compute translation and rotation errors for KITTI datasets. See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.

Parameters
poses_gt,GroundTruth poses
poses_result,Estimatedposes
t_err,Outputtranslation error (%)
r_err,Outputrotation error (deg/m)

Definition at line 649 of file Graph.cpp.

◆ calcRelativeErrors()

void rtabmap::graph::calcRelativeErrors ( const std::vector< Transform > &  poses_gt,
const std::vector< Transform > &  poses_result,
float &  t_err,
float &  r_err 
)

Compute average of translation and rotation errors between each poses.

Parameters
poses_gt,GroundTruth poses
poses_result,Estimatedposes
t_err,Outputtranslation error (m)
r_err,Outputrotation error (deg)

Definition at line 717 of file Graph.cpp.

◆ calcRMSE()

Transform rtabmap::graph::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 
)

Compute root-mean-square error (RMSE) like the TUM RGBD dataset's evaluation tool (absolute trajectory error). See https://vision.in.tum.de/data/datasets/rgbd-dataset

Parameters
groundTruth,GroundTruth poses
poses,Estimatedposes
Returns
Gt to Map transform

Definition at line 759 of file Graph.cpp.

◆ computeMaxGraphErrors()

void rtabmap::graph::computeMaxGraphErrors ( const std::map< int, Transform > &  poses,
const std::multimap< int, Link > &  links,
float &  maxLinearErrorRatio,
float &  maxAngularErrorRatio,
float &  maxLinearError,
float &  maxAngularError,
const Link **  maxLinearErrorLink = 0,
const Link **  maxAngularErrorLink = 0,
bool  for3DoF = false 
)

Definition at line 896 of file Graph.cpp.

◆ computeMinMax()

void rtabmap::graph::computeMinMax ( const std::map< int, Transform > &  poses,
cv::Vec3f &  min,
cv::Vec3f &  max 
)

Definition at line 2329 of file Graph.cpp.

◆ computePath() [1/3]

std::list< std::pair< int, Transform > > rtabmap::graph::computePath ( const std::map< int, rtabmap::Transform > &  poses,
const std::multimap< int, int > &  links,
int  from,
int  to,
bool  updateNewCosts = false 
)

Perform A* path planning in the graph.

Parameters
posesThe graph's poses
linksThe graph's links (from node id -> to node id)
frominitial node
tofinal node
updateNewCostsKeep up-to-date costs while traversing the graph.
Returns
the path ids from id "from" to id "to" including initial and final nodes.

Definition at line 1658 of file Graph.cpp.

◆ computePath() [2/3]

std::list< int > RTABMAP_EXP rtabmap::graph::computePath ( const std::multimap< int, Link > &  links,
int  from,
int  to,
bool  updateNewCosts = false,
bool  useSameCostForAllLinks = false 
)

Perform Dijkstra path planning in the graph.

Parameters
posesThe graph's poses
linksThe graph's links (from node id -> to node id)
frominitial node
tofinal node
updateNewCostsKeep up-to-date costs while traversing the graph.
useSameCostForAllLinksIgnore distance between nodes
Returns
the path ids from id "from" to id "to" including initial and final nodes.

Definition at line 1764 of file Graph.cpp.

◆ computePath() [3/3]

std::list< std::pair< int, Transform > > rtabmap::graph::computePath ( int  fromId,
int  toId,
const Memory memory,
bool  lookInDatabase = true,
bool  updateNewCosts = false,
float  linearVelocity = 0.0f,
float  angularVelocity = 0.0f 
)

Perform Dijkstra path planning in the graph.

Parameters
fromIdinitial node
toIdfinal node
memoryThe graph's memory
lookInDatabasecheck links in database
updateNewCostsKeep up-to-date costs while traversing the graph.
Returns
the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).

Definition at line 1866 of file Graph.cpp.

◆ computePathLength() [1/2]

float rtabmap::graph::computePathLength ( const std::vector< std::pair< int, Transform > > &  path,
unsigned int  fromIndex = 0,
unsigned int  toIndex = 0 
)

Definition at line 2248 of file Graph.cpp.

◆ computePathLength() [2/2]

float rtabmap::graph::computePathLength ( const std::map< int, Transform > &  path)

Definition at line 2273 of file Graph.cpp.

◆ exportGPS()

bool rtabmap::graph::exportGPS ( const std::string filePath,
const std::map< int, GPS > &  gpsValues,
unsigned int  rgba = 0xFFFFFFFF 
)

Definition at line 497 of file Graph.cpp.

◆ exportPoses()

bool rtabmap::graph::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>(),
const ParametersMap parameters = ParametersMap() 
)

Definition at line 54 of file Graph.cpp.

◆ filterDuplicateLinks()

std::multimap< int, Link > rtabmap::graph::filterDuplicateLinks ( const std::multimap< int, Link > &  links)

Definition at line 1140 of file Graph.cpp.

◆ filterLinks() [1/2]

std::multimap< int, Link > rtabmap::graph::filterLinks ( const std::multimap< int, Link > &  links,
Link::Type  filteredType,
bool  inverted = false 
)

Return links not of type "filteredType". If inverted=true, return links of type "filteredType".

Definition at line 1154 of file Graph.cpp.

◆ filterLinks() [2/2]

std::map< int, Link > rtabmap::graph::filterLinks ( const std::map< int, Link > &  links,
Link::Type  filteredType,
bool  inverted = false 
)

Return links not of type "filteredType". If inverted=true, return links of type "filteredType".

Definition at line 1179 of file Graph.cpp.

◆ findLink() [1/4]

std::multimap< int, Link >::iterator rtabmap::graph::findLink ( std::multimap< int, Link > &  links,
int  from,
int  to,
bool  checkBothWays = true,
Link::Type  type = Link::kUndef 
)

Definition at line 992 of file Graph.cpp.

◆ findLink() [2/4]

std::multimap< int, int >::iterator rtabmap::graph::findLink ( std::multimap< int, int > &  links,
int  from,
int  to,
bool  checkBothWays = true 
)

Definition at line 1025 of file Graph.cpp.

◆ findLink() [3/4]

std::multimap< int, Link >::const_iterator rtabmap::graph::findLink ( const std::multimap< int, Link > &  links,
int  from,
int  to,
bool  checkBothWays = true,
Link::Type  type = Link::kUndef 
)

Definition at line 1056 of file Graph.cpp.

◆ findLink() [4/4]

std::multimap< int, int >::const_iterator rtabmap::graph::findLink ( const std::multimap< int, int > &  links,
int  from,
int  to,
bool  checkBothWays = true 
)

Definition at line 1089 of file Graph.cpp.

◆ findLinks()

std::list< Link > rtabmap::graph::findLinks ( const std::multimap< int, Link > &  links,
int  from 
)

Definition at line 1121 of file Graph.cpp.

◆ findNearestNode()

int rtabmap::graph::findNearestNode ( const std::map< int, rtabmap::Transform > &  poses,
const rtabmap::Transform targetPose,
float *  distance = 0 
)

Find the nearest node of the target pose

Parameters
nodesthe nodes to search for
targetPosethe target pose to search around
distancesquared distance of the nearest node found (optional)
Returns
the node id.

Definition at line 2085 of file Graph.cpp.

◆ findNearestNodes() [1/3]

std::map< int, float > rtabmap::graph::findNearestNodes ( int  nodeId,
const std::map< int, Transform > &  poses,
float  radius,
float  angle = 0.0f,
int  k = 0 
)

Find the nearest nodes of the query pose or node

Parameters
nodeIdthe query id
nodesthe nodes to search for
radiusradius to search for (m), if 0, k should be > 0.
kmax nearest neighbors (0=all inside the radius)
Returns
the nodes with squared distance to query node.

Definition at line 2104 of file Graph.cpp.

◆ findNearestNodes() [2/3]

std::map< int, float > rtabmap::graph::findNearestNodes ( const Transform targetPose,
const std::map< int, Transform > &  poses,
float  radius,
float  angle = 0.0f,
int  k = 0 
)

Definition at line 2120 of file Graph.cpp.

◆ findNearestNodes() [3/3]

std::map<int, float> rtabmap::graph::findNearestNodes ( const std::map< int, rtabmap::Transform > &  nodes,
const rtabmap::Transform targetPose,
int  k 
)

Definition at line 2226 of file Graph.cpp.

◆ findNearestPoses() [1/2]

std::map< int, Transform > rtabmap::graph::findNearestPoses ( int  nodeId,
const std::map< int, Transform > &  poses,
float  radius,
float  angle = 0.0f,
int  k = 0 
)

Definition at line 2193 of file Graph.cpp.

◆ findNearestPoses() [2/2]

std::map< int, Transform > rtabmap::graph::findNearestPoses ( const Transform targetPose,
const std::map< int, Transform > &  poses,
float  radius,
float  angle = 0.0f,
int  k = 0 
)

Definition at line 2208 of file Graph.cpp.

◆ frustumPosesFiltering()

std::map< int, Transform > rtabmap::graph::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 at line 1204 of file Graph.cpp.

◆ getMaxOdomInf()

std::vector< double > rtabmap::graph::getMaxOdomInf ( const std::multimap< int, Link > &  links)

Definition at line 964 of file Graph.cpp.

◆ getNodesInRadius() [1/2]

std::map<int, float> rtabmap::graph::getNodesInRadius ( int  nodeId,
const std::map< int, Transform > &  nodes,
float  radius 
)

Definition at line 2230 of file Graph.cpp.

◆ getNodesInRadius() [2/2]

std::map<int, float> rtabmap::graph::getNodesInRadius ( const Transform targetPose,
const std::map< int, Transform > &  nodes,
float  radius 
)

Definition at line 2234 of file Graph.cpp.

◆ getPaths()

std::list< std::map< int, Transform > > rtabmap::graph::getPaths ( std::map< int, Transform poses,
const std::multimap< int, Link > &  links 
)

Definition at line 2297 of file Graph.cpp.

◆ getPosesInRadius() [1/2]

std::map<int, Transform> rtabmap::graph::getPosesInRadius ( int  nodeId,
const std::map< int, Transform > &  nodes,
float  radius,
float  angle 
)

Definition at line 2238 of file Graph.cpp.

◆ getPosesInRadius() [2/2]

std::map<int, Transform> rtabmap::graph::getPosesInRadius ( const Transform targetPose,
const std::map< int, Transform > &  nodes,
float  radius,
float  angle 
)

Definition at line 2242 of file Graph.cpp.

◆ importPoses()

bool rtabmap::graph::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 at line 184 of file Graph.cpp.

◆ lastFrameFromSegmentLength()

int32_t rtabmap::graph::lastFrameFromSegmentLength ( std::vector< float > &  dist,
int32_t  first_frame,
float  len 
)

Definition at line 627 of file Graph.cpp.

◆ radiusPosesClustering()

std::multimap< int, int > rtabmap::graph::radiusPosesClustering ( const std::map< int, Transform > &  poses,
float  radius,
float  angle 
)

Get all neighbor nodes in a fixed radius around each pose.

Parameters
posesThe poses
radiusRadius (m) of the search for near neighbors
angleMaximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
Returns
A map between each pose id and its neighbors found in the radius

Definition at line 1365 of file Graph.cpp.

◆ radiusPosesFiltering()

std::map< int, Transform > rtabmap::graph::radiusPosesFiltering ( const std::map< int, Transform > &  poses,
float  radius,
float  angle,
bool  keepLatest = true 
)

Get only the the most recent or older poses in the defined radius.

Parameters
posesThe poses
radiusRadius (m) of the search for near neighbors
angleMaximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
keepLatestkeep the latest node if true, otherwise the oldest node is kept
Returns
A map containing only most recent or older poses in the the defined radius

Definition at line 1253 of file Graph.cpp.

◆ reduceGraph()

void rtabmap::graph::reduceGraph ( const std::map< int, Transform > &  poses,
const std::multimap< int, Link > &  links,
std::multimap< int, int > &  hyperNodes,
std::multimap< int, Link > &  hyperLinks 
)

Definition at line 1421 of file Graph.cpp.

◆ rotationError()

float rtabmap::graph::rotationError ( const Transform pose_error)
inline

Definition at line 634 of file Graph.cpp.

◆ RTABMAP_DEPRECATED() [1/5]

rtabmap::graph::RTABMAP_DEPRECATED ( _mapIntFloat RTABMAP_EXP   findNearestNodesconst std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k,
"Use new findNearestNodes() interface with  radius = 0 
)

◆ RTABMAP_DEPRECATED() [2/5]

rtabmap::graph::RTABMAP_DEPRECATED ( _mapIntFloat RTABMAP_EXP   getNodesInRadiusint nodeId, const std::map< int, Transform > &nodes, float radius,
"Renamed to findNearestNodes()"   
)

◆ RTABMAP_DEPRECATED() [3/5]

rtabmap::graph::RTABMAP_DEPRECATED ( _mapIntFloat RTABMAP_EXP   getNodesInRadiusconst Transform &targetPose, const std::map< int, Transform > &nodes, float radius,
"Renamed to findNearestNodes()"   
)

◆ RTABMAP_DEPRECATED() [4/5]

rtabmap::graph::RTABMAP_DEPRECATED ( _mapIntTransform RTABMAP_EXP   getPosesInRadiusint nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f,
"Renamed to findNearestNodes()"   
)

◆ RTABMAP_DEPRECATED() [5/5]

rtabmap::graph::RTABMAP_DEPRECATED ( _mapIntTransform RTABMAP_EXP   getPosesInRadiusconst Transform &targetPose, const std::map< int, Transform > &nodes, float radius, float angle=0.0f,
"Renamed to findNearestNodes()"   
)

◆ trajectoryDistances()

std::vector<float> rtabmap::graph::trajectoryDistances ( const std::vector< Transform > &  poses)

Definition at line 613 of file Graph.cpp.

◆ translationError()

float rtabmap::graph::translationError ( const Transform pose_error)
inline

Definition at line 642 of file Graph.cpp.

Variable Documentation

◆ lengths

float rtabmap::graph::lengths[] = {100,200,300,400,500,600,700,800}

Definition at line 600 of file Graph.cpp.

◆ num_lengths

int32_t rtabmap::graph::num_lengths = 8

Definition at line 601 of file Graph.cpp.



rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:39:00