#include <fake_loop_closure.h>
Public Member Functions | |
FakeLoopClosureGenerator (double proximity_threshold) | |
NodeConstraintVector | getConstraints (const pose_graph::PoseGraph &graph, const pose_graph::NodeId new_node, const pose_graph::NodePoseMap &optimized_poses) |
Return a set of constraints given the current state of the graph the identity of the new node, and current optimized poses. | |
void | initializeFromGraph (const pose_graph::PoseGraph &g, const std::vector< pose_graph::NodeId > &node_sequence) |
void | updateLocalization (const graph_slam::GraphLocalization &loc) |
Private Member Functions | |
bool | haveRecentlyReceivedPose () const |
void | locCB (const geometry_msgs::PoseWithCovarianceStamped &msg) |
Private Attributes | |
ros::Subscriber | fake_loc_sub_ |
boost::optional < geometry_msgs::PoseStamped > | last_pose_ |
ros::NodeHandle | nh_ |
const double | proximity_threshold_ |
NodePoseMap | true_poses_ |
Definition at line 50 of file fake_loop_closure.h.
graph_slam::FakeLoopClosureGenerator::FakeLoopClosureGenerator | ( | double | proximity_threshold | ) |
Definition at line 52 of file fake_loop_closure.cpp.
NodeConstraintVector graph_slam::FakeLoopClosureGenerator::getConstraints | ( | const pose_graph::PoseGraph & | graph, |
const pose_graph::NodeId | new_node, | ||
const pose_graph::NodePoseMap & | optimized_poses | ||
) | [virtual] |
Return a set of constraints given the current state of the graph the identity of the new node, and current optimized poses.
Implements graph_slam::ConstraintGenerator.
Definition at line 67 of file fake_loop_closure.cpp.
bool graph_slam::FakeLoopClosureGenerator::haveRecentlyReceivedPose | ( | ) | const [private] |
Definition at line 104 of file fake_loop_closure.cpp.
void graph_slam::FakeLoopClosureGenerator::initializeFromGraph | ( | const pose_graph::PoseGraph & | g, |
const std::vector< pose_graph::NodeId > & | node_sequence | ||
) |
Definition at line 57 of file fake_loop_closure.cpp.
void graph_slam::FakeLoopClosureGenerator::locCB | ( | const geometry_msgs::PoseWithCovarianceStamped & | msg | ) | [private] |
Definition at line 96 of file fake_loop_closure.cpp.
void graph_slam::FakeLoopClosureGenerator::updateLocalization | ( | const graph_slam::GraphLocalization & | loc | ) | [virtual] |
Implements graph_slam::ConstraintGenerator.
Definition at line 62 of file fake_loop_closure.cpp.
Definition at line 70 of file fake_loop_closure.h.
boost::optional<geometry_msgs::PoseStamped> graph_slam::FakeLoopClosureGenerator::last_pose_ [private] |
Definition at line 82 of file fake_loop_closure.h.
Definition at line 69 of file fake_loop_closure.h.
const double graph_slam::FakeLoopClosureGenerator::proximity_threshold_ [private] |
Definition at line 76 of file fake_loop_closure.h.
Definition at line 83 of file fake_loop_closure.h.