Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | Friends
graph_slam::ScanManager Class Reference

Internally used class that contains sequential scan matching logic When using the ops, you have to instantiate a WithOptimizedPoses RAII object in a containing scope to set the optimized poses before doing anything else. More...

#include <scan_manager.h>

Inheritance diagram for graph_slam::ScanManager:
Inheritance graph
[legend]

List of all members.

Public Member Functions

void addNode (pg::NodeId n, LaserPtr scan)
pg::NodeId closestNode (const geometry_msgs::Pose2D &pose, const NodeSet &nodes) const
geometry_msgs::Point computeBarycenterInBaseFrame (const LaserPtr scan) const
boost::optional< pg::NodeIdlastNode () const
 Return the previous node if it exists, or an uninitialized object otherwise.
NodeSet nearbyNodes (pg::NodeId n, const geometry_msgs::Point &center, const pg::PoseGraph &graph, const double max_distance) const
void resetLastNode ()
 Reset the state so the last node becomes uninitialized.
 ScanManager (const geometry_msgs::Pose2D &laser_offset)

Protected Member Functions

Chain getChain (pg::NodeId n, const NodeSet &nodes, const NodeSet &processed) const
template<typename NodeContainer >
std::vector
< karto_scan_matcher::ScanWithPose
makeRefScans (const NodeContainer &nodes) const
 Return ScanWithPose objects for the given nodes, that can be passed to the Karto scan matcher.
boost::optional
< geometry_msgs::Point
nodeBarycenter (const pg::NodeId n) const
geometry_msgs::Pose optimizedPose (const pg::NodeId n) const
 Return the currently stored optimized pose of the node (as set by WithOptimizedPoses)
NodeConstraint scanChainConstraint (const geometry_msgs::Point &barycenter, const geometry_msgs::Pose2D &pose, const pg::PrecisionMatrix &prec, const Chain &chain) const
karto_scan_matcher::ScanMatchResult scanMatchNodes (KartoMatcherPtr matcher, const geometry_msgs::Pose &init_pose_estimate, const LaserPtr scan, const NodeSet &nodes) const
 Perform scan matching against nodes given a new scan, initial estimate.

Protected Attributes

const geometry_msgs::Pose2D laser_offset_
 The offset of the laser from the base.
NodeSequenceMap node_sequence_
 The sequential order of nodes.

Private Member Functions

pg::NodeId closestNode (const geometry_msgs::Point &barycenter, const Chain &chain) const
 Return node in chain whose scan barycenter is closest to barycenter.
geometry_msgs::Point nodeBarycenter (const pg::NodeId n, const geometry_msgs::Pose &pose) const
 Return the barycenter of the scan at n assuming its optimized pose is pose.
void setOptimizedPoses (const NodePoseMap &optimized_poses) const
 Allow temporarily setting optimized poses.
void unsetOptimizedPoses () const

Private Attributes

std::map< pg::NodeId,
geometry_msgs::Point
barycenters_
boost::optional< pg::NodeIdlast_node_
boost::optional< NodePoseMapoptimized_poses_
NodeScanMap scans_

Friends

class WithOptimizedPoses

Detailed Description

Internally used class that contains sequential scan matching logic When using the ops, you have to instantiate a WithOptimizedPoses RAII object in a containing scope to set the optimized poses before doing anything else.

Definition at line 67 of file scan_manager.h.


Constructor & Destructor Documentation

graph_slam::ScanManager::ScanManager ( const geometry_msgs::Pose2D &  laser_offset)

Definition at line 63 of file scan_manager.cpp.


Member Function Documentation

Stores the node's scan, precomputes the barycenter, and keeps track of the sequential order of nodes

Definition at line 66 of file scan_manager.cpp.

pg::NodeId graph_slam::ScanManager::closestNode ( const geometry_msgs::Pose2D &  pose,
const NodeSet nodes 
) const

Return the node whose optimized pose is closest to pose Ignore any nodes without an optimized pose Assert if there isn't at least one node with an optimized pose

pg::NodeId graph_slam::ScanManager::closestNode ( const geometry_msgs::Point barycenter,
const Chain chain 
) const [private]

Return node in chain whose scan barycenter is closest to barycenter.

Return the barycenter of the given laser scan, in the base frame (as opposed to the laser's sensor frame)

Definition at line 126 of file scan_manager.cpp.

Chain graph_slam::ScanManager::getChain ( pg::NodeId  n,
const NodeSet nodes,
const NodeSet processed 
) const [protected]

Go forward and backward from n in the sequential ordering of nodes, and return the maximal contiguous chain of nodes that are all in nodes but not in processed. n itself must satisfy this description.

Definition at line 112 of file scan_manager.cpp.

Return the previous node if it exists, or an uninitialized object otherwise.

Definition at line 88 of file scan_manager.cpp.

template<typename NodeContainer >
vector< ksm::ScanWithPose > graph_slam::ScanManager::makeRefScans ( const NodeContainer &  nodes) const [protected]

Return ScanWithPose objects for the given nodes, that can be passed to the Karto scan matcher.

Definition at line 101 of file scan_manager.cpp.

NodeSet graph_slam::ScanManager::nearbyNodes ( pg::NodeId  n,
const geometry_msgs::Point center,
const pg::PoseGraph graph,
const double  max_distance 
) const

Find the graph neighborhood of n, of nodes whose scan barycenter in the global frame is within max_distance of center. Skip nodes that don't have a scan or don't have an optimized pose.

Definition at line 148 of file scan_manager.cpp.

optional< gm::Point > graph_slam::ScanManager::nodeBarycenter ( const pg::NodeId  n) const [protected]

Return the barycenter of the given node's scan in the optimized frame. Returns an empty object if either the node has no optimized frame, or no attached scan.

Definition at line 134 of file scan_manager.cpp.

Return the barycenter of the scan at n assuming its optimized pose is pose.

Return the currently stored optimized pose of the node (as set by WithOptimizedPoses)

Definition at line 239 of file scan_manager.cpp.

Reset the state so the last node becomes uninitialized.

Definition at line 81 of file scan_manager.cpp.

NodeConstraint graph_slam::ScanManager::scanChainConstraint ( const geometry_msgs::Point barycenter,
const geometry_msgs::Pose2D &  pose,
const pg::PrecisionMatrix prec,
const Chain chain 
) const [protected]

Return a constraint from a new node at pose to the given chain. The constraint is to the node whose scan barycenter is closest to barycenter

Definition at line 180 of file scan_manager.cpp.

ksm::ScanMatchResult graph_slam::ScanManager::scanMatchNodes ( KartoMatcherPtr  matcher,
const geometry_msgs::Pose init_pose_estimate,
const LaserPtr  scan,
const NodeSet nodes 
) const [protected]

Perform scan matching against nodes given a new scan, initial estimate.

Definition at line 244 of file scan_manager.cpp.

void graph_slam::ScanManager::setOptimizedPoses ( const NodePoseMap optimized_poses) const [private]

Allow temporarily setting optimized poses.

Definition at line 276 of file scan_manager.cpp.

Definition at line 281 of file scan_manager.cpp.


Friends And Related Function Documentation

friend class WithOptimizedPoses [friend]

Definition at line 151 of file scan_manager.h.


Member Data Documentation

Definition at line 137 of file scan_manager.h.

const geometry_msgs::Pose2D graph_slam::ScanManager::laser_offset_ [protected]

The offset of the laser from the base.

Definition at line 122 of file scan_manager.h.

boost::optional<pg::NodeId> graph_slam::ScanManager::last_node_ [private]

Definition at line 138 of file scan_manager.h.

The sequential order of nodes.

Definition at line 125 of file scan_manager.h.

boost::optional<NodePoseMap> graph_slam::ScanManager::optimized_poses_ [mutable, private]

Definition at line 139 of file scan_manager.h.

Definition at line 136 of file scan_manager.h.


The documentation for this class was generated from the following files:


graph_slam
Author(s): Bhaskara Marthi
autogenerated on Tue Jan 7 2014 11:17:21