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

Classes

struct  errors
 
class  Node
 
struct  Order
 

Typedefs

typedef std::pair< int, float > Pair
 

Functions

void RTABMAP_CORE_EXPORT calcKittiSequenceErrors (const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
 
void RTABMAP_CORE_EXPORT calcRelativeErrors (const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
 
Transform RTABMAP_CORE_EXPORT 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_CORE_EXPORT 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_CORE_EXPORT computeMinMax (const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
 
std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT 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_CORE_EXPORT computePath (const std::multimap< int, Link > &links, int from, int to, bool updateNewCosts=false, bool useSameCostForAllLinks=false)
 
std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT computePath (int fromId, int toId, const Memory *memory, bool lookInDatabase=true, bool updateNewCosts=false, float linearVelocity=0.0f, float angularVelocity=0.0f)
 
float RTABMAP_CORE_EXPORT computePathLength (const std::map< int, Transform > &path)
 
float RTABMAP_CORE_EXPORT computePathLength (const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
 
bool RTABMAP_CORE_EXPORT exportGPS (const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
 
bool RTABMAP_CORE_EXPORT 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_CORE_EXPORT filterDuplicateLinks (const std::multimap< int, Link > &links)
 
std::map< int, Link > RTABMAP_CORE_EXPORT filterLinks (const std::map< int, Link > &links, Link::Type filteredType, bool inverted=false)
 
std::multimap< int, Link > RTABMAP_CORE_EXPORT filterLinks (const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
 
std::multimap< int, int >::const_iterator RTABMAP_CORE_EXPORT findLink (const std::multimap< int, int > &links, int from, int to, bool checkBothWays=true)
 
std::multimap< int, Link >::const_iterator RTABMAP_CORE_EXPORT findLink (const std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
 
std::multimap< int, std::pair< int, Link::Type > >::const_iterator RTABMAP_CORE_EXPORT findLink (const std::multimap< int, std::pair< int, Link::Type > > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
 
std::multimap< int, int >::iterator RTABMAP_CORE_EXPORT findLink (std::multimap< int, int > &links, int from, int to, bool checkBothWays=true)
 
std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT findLink (std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
 
std::multimap< int, std::pair< int, Link::Type > >::iterator RTABMAP_CORE_EXPORT findLink (std::multimap< int, std::pair< int, Link::Type > > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
 
std::list< Link > RTABMAP_CORE_EXPORT findLinks (const std::multimap< int, Link > &links, int from)
 
int RTABMAP_CORE_EXPORT findNearestNode (const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
 
RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT findNearestNodes (const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &targetPose, int k)
 
std::map< int, float > RTABMAP_CORE_EXPORT findNearestNodes (const Transform &targetPose, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, float > RTABMAP_CORE_EXPORT findNearestNodes (int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, Transform > RTABMAP_CORE_EXPORT findNearestPoses (const Transform &targetPose, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, Transform > RTABMAP_CORE_EXPORT findNearestPoses (int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
 
std::map< int, Transform > RTABMAP_CORE_EXPORT 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_CORE_EXPORT getMaxOdomInf (const std::multimap< int, Link > &links)
 
RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT getNodesInRadius (const Transform &targetPose, const std::map< int, Transform > &nodes, float radius)
 
RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT getNodesInRadius (int nodeId, const std::map< int, Transform > &nodes, float radius)
 
std::list< std::map< int, Transform > > RTABMAP_CORE_EXPORT getPaths (std::map< int, Transform > poses, const std::multimap< int, Link > &links)
 
RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT getPosesInRadius (const Transform &targetPose, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)
 
RTABMAP_DEPRECATED std::map< int, Transform > RTABMAP_CORE_EXPORT getPosesInRadius (int nodeId, const std::map< int, Transform > &nodes, float radius, float angle=0.0f)
 
bool RTABMAP_CORE_EXPORT 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_CORE_EXPORT radiusPosesClustering (const std::map< int, Transform > &poses, float radius, float angle)
 
std::map< int, Transform > RTABMAP_CORE_EXPORT 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)
 
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

◆ Pair

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

Definition at line 1746 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 2427 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 1756 of file Graph.cpp.

◆ computePath() [2/3]

std::list< int > 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 1862 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 1964 of file Graph.cpp.

◆ computePathLength() [1/2]

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

Definition at line 2371 of file Graph.cpp.

◆ computePathLength() [2/2]

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

Definition at line 2346 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 1238 of file Graph.cpp.

◆ filterLinks() [1/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 1277 of file Graph.cpp.

◆ filterLinks() [2/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 1252 of file Graph.cpp.

◆ findLink() [1/6]

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 1187 of file Graph.cpp.

◆ findLink() [2/6]

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 1121 of file Graph.cpp.

◆ findLink() [3/6]

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

Definition at line 1154 of file Graph.cpp.

◆ findLink() [4/6]

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

Definition at line 1090 of file Graph.cpp.

◆ findLink() [5/6]

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 1024 of file Graph.cpp.

◆ findLink() [6/6]

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

Definition at line 1057 of file Graph.cpp.

◆ findLinks()

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

Definition at line 1219 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 2183 of file Graph.cpp.

◆ findNearestNodes() [1/3]

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

Definition at line 2324 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 2218 of file Graph.cpp.

◆ findNearestNodes() [3/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 2202 of file Graph.cpp.

◆ findNearestPoses() [1/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 2306 of file Graph.cpp.

◆ findNearestPoses() [2/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 2291 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 1302 of file Graph.cpp.

◆ getMaxOdomInf()

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

Definition at line 996 of file Graph.cpp.

◆ getNodesInRadius() [1/2]

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

Definition at line 2332 of file Graph.cpp.

◆ getNodesInRadius() [2/2]

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

Definition at line 2328 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 2395 of file Graph.cpp.

◆ getPosesInRadius() [1/2]

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

Definition at line 2340 of file Graph.cpp.

◆ getPosesInRadius() [2/2]

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

Definition at line 2336 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 1463 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 1351 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 1519 of file Graph.cpp.

◆ rotationError()

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

Definition at line 634 of file Graph.cpp.

◆ 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 Thu Jul 25 2024 02:50:28