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>
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::NodeId > | lastNode () const |
| Return the previous node if it exists, or an uninitialized object otherwise. | |
| NodeSet | nearbyNodes (pg::NodeId n, const geometry_msgs::Point ¢er, 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::NodeId > | last_node_ |
| boost::optional< NodePoseMap > | optimized_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
| void graph_slam::ScanManager::addNode | ( | pg::NodeId | n, |
| LaserPtr | scan | ||
| ) |
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.
| gm::Point graph_slam::ScanManager::computeBarycenterInBaseFrame | ( | const LaserPtr | scan | ) | const |
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.
| optional< NodeId > graph_slam::ScanManager::lastNode | ( | ) | const |
Return the previous node if it exists, or an uninitialized object otherwise.
Definition at line 88 of file scan_manager.cpp.
| 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.
| geometry_msgs::Point graph_slam::ScanManager::nodeBarycenter | ( | const pg::NodeId | n, |
| const geometry_msgs::Pose & | pose | ||
| ) | const [private] |
Return the barycenter of the scan at n assuming its optimized pose is pose.
| geometry_msgs::Pose graph_slam::ScanManager::optimizedPose | ( | const pg::NodeId | n | ) | const [protected] |
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.
| void graph_slam::ScanManager::unsetOptimizedPoses | ( | ) | const [private] |
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
std::map<pg::NodeId, geometry_msgs::Point> graph_slam::ScanManager::barycenters_ [private] |
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.
NodeScanMap graph_slam::ScanManager::scans_ [private] |
Definition at line 136 of file scan_manager.h.
The documentation for this class was generated from the following files: