Computes a globally optimal trajectory from transformations between Node-pairs. More...
#include <graph_manager.h>
Public Slots | |
void | clearPointCloud (pointcloud_type const *pc) |
void | clearPointClouds () |
void | deleteLastFrame () |
Throw the last node out, reoptimize. | |
void | filterNodesByPosition (float x, float y, float z) |
void | occupancyFilterClouds () |
Remove points in free space for all nodes (requires octomapping to be done) | |
double | optimizeGraph (double iter=-1, bool nonthreaded=false) |
void | printEdgeErrors (QString) |
unsigned int | pruneEdgesWithErrorAbove (float) |
void | reducePointCloud (pointcloud_type const *pc) |
void | reset () |
Start over with new graph. | |
void | sanityCheck (float) |
void | saveAllClouds (QString filename, bool threaded=true) |
Call saveAllCloudsToFile, as background thread if threaded=true and possible. | |
void | saveAllFeatures (QString filename, bool threaded=true) |
Call saveAllFeaturesToFile, as background thread if threaded=true and possible. | |
void | saveBagfile (QString filename) |
iterate over all Nodes, writing their transform and pointcloud to a bagfile | |
void | saveBagfileAsync (QString filename) |
void | saveG2OGraph (QString filename) |
Save graph in g2o format. | |
void | saveIndividualClouds (QString file_basename, bool threaded=true) |
Call saveIndividualCloudsToFile, as background thread if threaded=true and possible. | |
void | saveOctomap (QString filename, bool threaded=true) |
Save the cloud as octomap by using the (internal) octomap server to render the point clouds into the voxel map. | |
void | saveTrajectory (QString filebasename, bool with_ground_truth=false) |
Save trajectory of computed motion and ground truth (if available) | |
void | sendAllClouds (bool threaded_if_available=true) |
iterate over all Nodes, sending their transform and pointcloud | |
void | toggleMapping (bool) |
Signals | |
void | deleteLastNode () |
void | iamBusy (int id, const char *message, int max) |
void | newTransformationMatrix (QString) |
Connect to this signal to get the transformation matrix from the last frame as QString. | |
void | progress (int id, const char *message, int val) |
void | renderableOctomap (Renderable *r) |
void | resetGLViewer () |
void | sendFinished () |
void | setFeatures (const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > *) |
void | setGraph (const g2o::OptimizableGraph *) |
void | setGraphEdges (const QList< QPair< int, int > > *edge_list) |
void | setGUIInfo (QString message) |
void | setGUIInfo2 (QString message) |
void | setGUIStatus (QString message) |
void | setPointCloud (pointcloud_type *pc, QMatrix4x4 transformation) |
void | updateTransforms (QList< QMatrix4x4 > *transformations) |
Public Member Functions | |
bool | addNode (Node *newNode) |
void | addOdometry (ros::Time timestamp, tf::TransformListener *listener) |
void | deleteFeatureInformation () |
void | drawFeatureFlow (cv::Mat &canvas, cv::Scalar line_color=cv::Scalar(255, 0, 0, 0), cv::Scalar circle_color=cv::Scalar(0, 0, 255, 0)) |
Draw the features's motions onto the canvas. | |
void | drawFeatureFlow (cv::Mat &canvas_flow, cv::Mat &canvas_features, cv::Scalar line_color=cv::Scalar(255, 0, 0, 0), cv::Scalar circle_color=cv::Scalar(0, 0, 255, 0)) |
void | firstNode (Node *new_node) |
Adds the first node. | |
GraphManager () | |
bool | isBusy () |
Used by OpenNIListener. Indicates whether long running computations are running. | |
bool | nodeComparisons (Node *newNode, QMatrix4x4 &curr_motion_estimate, bool &edge_to_keyframe) |
Output:contains the best-yet of the pairwise motion estimates for the current node. | |
void | setEmpiricalCovariances () |
void | setOptimizerVerbose (bool verbose) |
void | writeOctomap (QString filename) const |
Only write (not create, not clear) existing octomap. | |
~GraphManager () | |
Public Attributes | |
g2o::HyperGraph::EdgeSet | cam_cam_edges_ |
"Regular" edges from camera to camera as found form feature correspondeces | |
g2o::HyperGraph::VertexSet | camera_vertices |
Pose vertices (in camera coordinate system) | |
g2o::HyperGraph::EdgeSet | current_match_edges_ |
g2o::HyperGraph::EdgeSet | odometry_edges_ |
Edges added by addOdometryEdgeToG2O(...) | |
Protected Types | |
typedef std::pair< int, Node * > | GraphNodeType |
Map from node id to node. Assumption is, that ids start at 0 and are consecutive. | |
Protected Member Functions | |
bool | addEdgeToG2O (const LoadedEdge3D &edge, Node *n1, Node *n2, bool good_edge, bool set_estimate, QMatrix4x4 &motion_estimate) |
Adds an visual-feature-correspondence edge to the optimizer. | |
void | addKeyframe (int id) |
Add a keyframe to the list (and log keyframes) | |
bool | addOdometryEdgeToG2O (const LoadedEdge3D &edge, Node *n1, Node *n2, QMatrix4x4 &motion_estimate) |
Adds an odometry edge to the optimizer. | |
void | broadcastLatestTransform (const ros::TimerEvent &event) const |
Broadcast cached transform. | |
void | broadcastTransform (const tf::StampedTransform &computed_motion) |
Broadcast given transform. | |
tf::StampedTransform | computeFixedToBaseTransform (Node *node, bool invert) |
Compute the tranformation between (sensor) Base and Fixed (Map) frame. | |
void | createOptimizer (std::string backend, g2o::SparseOptimizer *optimizer=NULL) |
Instanciate the optimizer with the desired backend. | |
void | deleteCameraFrame (int id) |
double | geodesicDiscount (g2o::HyperDijkstra &hypdij, const MatchingResult &mr) |
QList< QMatrix4x4 > * | getAllPosesAsMatrixList () const |
Return pointer to a list of the optimizers graph poses on the heap(!) | |
QList< QPair< int, int > > * | getGraphEdges () |
Return pointer to a list of the optimizers graph edges on the heap(!) | |
QList< int > | getPotentialEdgeTargetsWithDijkstra (const Node *new_node, int sequential_targets, int geodesic_targets, int sampled_targets=5, int predecessor_id=-1, bool include_predecessor=false) |
int | last_added_cam_vertex_id () |
void | localizationUpdate (Node *new_node, QMatrix4x4 motion_estimate) |
use matching results to update location | |
int | nodeId2VertexId (int node_id) |
double | optimizeGraphImpl (double max_iter) |
Applies g2o for optimization returns chi2. | |
void | pointCloud2MeshFile (QString filename, pointcloud_type full_cloud) |
void | renderToOctomap (Node *node) |
void | resetGraph () |
Start over. | |
void | saveAllCloudsToFile (QString filename) |
iterate over all Nodes, transform them to the fixed frame, aggregate and save | |
void | saveAllFeaturesToFile (QString filename) |
Transform all feature positions to global coordinates and save them together with the belonging descriptors. | |
void | saveIndividualCloudsToFile (QString filename) |
iterate over all Nodes, save each in one pcd-file | |
void | saveOctomapImpl (QString filename) |
void | savePlyFile (QString filename, pointcloud_normal_type &full_cloud) |
void | sendAllCloudsImpl () |
void | setEmpiricalCovariancesForEdgeSet (EdgeSet &edges) |
See setEmpiricalCovariances. | |
tf::StampedTransform | stampedTransformInWorldFrame (const Node *node, const tf::Transform &computed_motion) const |
bool | updateCloudOrigin (Node *node) |
void | visualizeFeatureFlow3D (unsigned int marker_id=0, bool draw_outlier=true) |
Send markers to visualize the last matched features in rviz (if somebody subscribed) | |
void | visualizeGraphEdges () const |
Send markers to visualize the graph edges (cam transforms) in rviz (if somebody subscribed) | |
void | visualizeGraphIds () const |
Send markers to visualize the graph ids in rviz (if somebody subscribed) | |
void | visualizeGraphNodes () const |
Send markers to visualize the graph nodes (cam positions) in rviz (if somebody subscribed) | |
Protected Attributes | |
ros::Publisher | batch_cloud_pub_ |
bool | batch_processing_runs_ |
tf::TransformBroadcaster | br_ |
Used to broadcast the pose estimate. | |
g2o::SparseOptimizer::Vertex * | calibration_vertex_ |
ColorOctomapServer | co_server_ |
tf::Transform | computed_motion_ |
transformation of the last frame to the first frame (assuming the first one is fixed) | |
MatchingResult | curr_best_result_ |
will contain the motion to the best matching node | |
std::string | current_backend_ |
QList< QPair< int, int > > | current_edges_ |
int | earliest_loop_closure_node_ |
std::map< int, Node * > | graph_ |
tf::Transform | init_base_pose_ |
QList< int > | keyframe_ids_ |
unsigned int | last_odom_time_ |
tf::StampedTransform | latest_transform_cache_ |
bool | localization_only_ |
unsigned int | loop_closures_edges |
unsigned int | marker_id_ |
ros::Publisher | marker_pub_ |
unsigned int | next_seq_id |
unsigned int | next_vertex_id |
ros::Publisher | online_cloud_pub_ |
QMutex | optimization_mutex_ |
This mutex restricts the optimization computation itself. | |
g2o::SparseOptimizer * | optimizer_ |
QMutex | optimizer_mutex_ |
This mutex restricts access to the optimizer's data structures. | |
bool | process_node_runs_ |
ros::Publisher | ransac_marker_pub_ |
bool | reset_request_ |
g2o::RobustKernelHuber | robust_kernel_ |
unsigned int | sequential_edges |
ros::Timer | timer_ |
Used to start the broadcasting of the pose estimate regularly. | |
ros::Publisher | whole_cloud_pub_ |
Computes a globally optimal trajectory from transformations between Node-pairs.
Definition at line 89 of file graph_manager.h.
typedef std::pair<int, Node*> GraphManager::GraphNodeType [protected] |
Map from node id to node. Assumption is, that ids start at 0 and are consecutive.
Definition at line 336 of file graph_manager.h.
Definition at line 63 of file graph_manager.cpp.
Definition at line 84 of file graph_manager2.cpp.
bool GraphManager::addEdgeToG2O | ( | const LoadedEdge3D & | edge, |
Node * | n1, | ||
Node * | n2, | ||
bool | good_edge, | ||
bool | set_estimate, | ||
QMatrix4x4 & | motion_estimate | ||
) | [protected] |
Adds an visual-feature-correspondence edge to the optimizer.
Definition at line 811 of file graph_manager.cpp.
void GraphManager::addKeyframe | ( | int | id | ) | [protected] |
Add a keyframe to the list (and log keyframes)
Definition at line 784 of file graph_manager.cpp.
bool GraphManager::addNode | ( | Node * | newNode | ) |
Add new node to the graph. Node will be included, if a valid transformation to one of the former nodes can be found. If appropriate, the graph is optimized graphmanager owns newNode after this call. Do no delete the object
Output:contains the best-yet of the pairwise motion estimates for the current node
Definition at line 681 of file graph_manager.cpp.
void GraphManager::addOdometry | ( | ros::Time | timestamp, |
tf::TransformListener * | listener | ||
) |
Check if all nodes include the odometry. If not, add a new edge.
Definition at line 62 of file graph_mgr_odom.cpp.
bool GraphManager::addOdometryEdgeToG2O | ( | const LoadedEdge3D & | edge, |
Node * | n1, | ||
Node * | n2, | ||
QMatrix4x4 & | motion_estimate | ||
) | [protected] |
Adds an odometry edge to the optimizer.
Definition at line 133 of file graph_mgr_odom.cpp.
void GraphManager::broadcastLatestTransform | ( | const ros::TimerEvent & | event | ) | const [protected] |
Broadcast cached transform.
Definition at line 953 of file graph_mgr_io.cpp.
void GraphManager::broadcastTransform | ( | const tf::StampedTransform & | computed_motion | ) | [protected] |
Broadcast given transform.
Definition at line 965 of file graph_mgr_io.cpp.
void GraphManager::clearPointCloud | ( | pointcloud_type const * | pc | ) | [slot] |
Definition at line 48 of file graph_manager2.cpp.
void GraphManager::clearPointClouds | ( | ) | [slot] |
Definition at line 41 of file graph_manager2.cpp.
tf::StampedTransform GraphManager::computeFixedToBaseTransform | ( | Node * | node, |
bool | invert | ||
) | [protected] |
Compute the tranformation between (sensor) Base and Fixed (Map) frame.
Definition at line 55 of file graph_mgr_io.cpp.
void GraphManager::createOptimizer | ( | std::string | backend, |
g2o::SparseOptimizer * | optimizer = NULL |
||
) | [protected] |
Instanciate the optimizer with the desired backend.
Definition at line 107 of file graph_manager.cpp.
void GraphManager::deleteCameraFrame | ( | int | id | ) | [protected] |
Definition at line 1077 of file graph_manager.cpp.
Warning: This is a dangerous way to save memory. Some methods will behave undefined after this. Notable exception: optimizeGraph()
Definition at line 103 of file graph_manager2.cpp.
void GraphManager::deleteLastFrame | ( | ) | [slot] |
Throw the last node out, reoptimize.
Definition at line 61 of file graph_manager2.cpp.
void GraphManager::deleteLastNode | ( | ) | [signal] |
void GraphManager::drawFeatureFlow | ( | cv::Mat & | canvas, |
cv::Scalar | line_color = cv::Scalar(255,0,0,0) , |
||
cv::Scalar | circle_color = cv::Scalar(0,0,255,0) |
||
) |
Draw the features's motions onto the canvas.
Definition at line 1055 of file graph_mgr_io.cpp.
void GraphManager::drawFeatureFlow | ( | cv::Mat & | canvas_flow, |
cv::Mat & | canvas_features, | ||
cv::Scalar | line_color = cv::Scalar(255,0,0,0) , |
||
cv::Scalar | circle_color = cv::Scalar(0,0,255,0) |
||
) |
Definition at line 1110 of file graph_mgr_io.cpp.
void GraphManager::filterNodesByPosition | ( | float | x, |
float | y, | ||
float | z | ||
) | [slot] |
Definition at line 1362 of file graph_manager.cpp.
void GraphManager::firstNode | ( | Node * | new_node | ) |
Adds the first node.
Output:contains the best-yet of the pairwise motion estimates for the current node
Definition at line 360 of file graph_manager.cpp.
double GraphManager::geodesicDiscount | ( | g2o::HyperDijkstra & | hypdij, |
const MatchingResult & | mr | ||
) | [protected] |
Definition at line 1336 of file graph_manager.cpp.
QList< QMatrix4x4 > * GraphManager::getAllPosesAsMatrixList | ( | ) | const [protected] |
Return pointer to a list of the optimizers graph poses on the heap(!)
Definition at line 1288 of file graph_manager.cpp.
QList< QPair< int, int > > * GraphManager::getGraphEdges | ( | ) | [protected] |
Return pointer to a list of the optimizers graph edges on the heap(!)
Definition at line 1248 of file graph_manager.cpp.
QList< int > GraphManager::getPotentialEdgeTargetsWithDijkstra | ( | const Node * | new_node, |
int | sequential_targets, | ||
int | geodesic_targets, | ||
int | sampled_targets = 5 , |
||
int | predecessor_id = -1 , |
||
bool | include_predecessor = false |
||
) | [protected] |
Suggest nodes for comparison. Suggests <sequential_targets> direct predecessors in the time sequence <geodesic_targets> nodes from the graph-neighborhood and <sample_targets> randomly chosen from the keyframes Using the graph neighborhood, has the advantage that once a loop closure is found by sampling, the edge will be used to find more closures, where the first one was found
Definition at line 204 of file graph_manager.cpp.
void GraphManager::iamBusy | ( | int | id, |
const char * | message, | ||
int | max | ||
) | [signal] |
bool GraphManager::isBusy | ( | ) |
Used by OpenNIListener. Indicates whether long running computations are running.
Definition at line 37 of file graph_manager2.cpp.
int GraphManager::last_added_cam_vertex_id | ( | ) | [inline, protected] |
Definition at line 296 of file graph_manager.h.
void GraphManager::localizationUpdate | ( | Node * | new_node, |
QMatrix4x4 | motion_estimate | ||
) | [protected] |
use matching results to update location
Definition at line 660 of file graph_manager.cpp.
void GraphManager::newTransformationMatrix | ( | QString | ) | [signal] |
Connect to this signal to get the transformation matrix from the last frame as QString.
bool GraphManager::nodeComparisons | ( | Node * | newNode, |
QMatrix4x4 & | curr_motion_estimate, | ||
bool & | edge_to_keyframe | ||
) |
Output:contains the best-yet of the pairwise motion estimates for the current node.
Try to compute transformations to previous nodes getPotentialEdgeTargetsWithDijkstra is used to select previous nodes to match against, then the comparison of nodes is called, possibly in parallel.
Definition at line 421 of file graph_manager.cpp.
int GraphManager::nodeId2VertexId | ( | int | node_id | ) | [inline, protected] |
Definition at line 300 of file graph_manager.h.
void GraphManager::occupancyFilterClouds | ( | ) | [slot] |
Remove points in free space for all nodes (requires octomapping to be done)
Definition at line 1372 of file graph_manager.cpp.
double GraphManager::optimizeGraph | ( | double | iter = -1 , |
bool | nonthreaded = false |
||
) | [slot] |
Calls optimizeGraphImpl either in fore- or background depending on parameter concurrent_optimization Returns chi2
Definition at line 900 of file graph_manager.cpp.
double GraphManager::optimizeGraphImpl | ( | double | max_iter | ) | [protected] |
Applies g2o for optimization returns chi2.
Definition at line 938 of file graph_manager.cpp.
void GraphManager::pointCloud2MeshFile | ( | QString | filename, |
pointcloud_type | full_cloud | ||
) | [protected] |
Definition at line 583 of file graph_mgr_io.cpp.
void GraphManager::printEdgeErrors | ( | QString | filename | ) | [slot] |
Definition at line 1321 of file graph_manager.cpp.
void GraphManager::progress | ( | int | id, |
const char * | message, | ||
int | val | ||
) | [signal] |
unsigned int GraphManager::pruneEdgesWithErrorAbove | ( | float | thresh | ) | [slot] |
Actually only discounts the edges drastically Returns the number of edges that have been discounted Thresh corresponds to a squared error
Actually only discount them drastically. Thresh corresponds to a squared error
Definition at line 1106 of file graph_manager.cpp.
void GraphManager::reducePointCloud | ( | pointcloud_type const * | pc | ) | [slot] |
Definition at line 1310 of file graph_manager.cpp.
void GraphManager::renderableOctomap | ( | Renderable * | r | ) | [signal] |
void GraphManager::renderToOctomap | ( | Node * | node | ) | [protected] |
Definition at line 317 of file graph_mgr_io.cpp.
void GraphManager::reset | ( | ) | [slot] |
Start over with new graph.
Definition at line 80 of file graph_manager2.cpp.
void GraphManager::resetGLViewer | ( | ) | [signal] |
void GraphManager::resetGraph | ( | ) | [protected] |
Start over.
Definition at line 326 of file graph_manager.cpp.
void GraphManager::sanityCheck | ( | float | thresh | ) | [slot] |
Definition at line 1347 of file graph_manager.cpp.
void GraphManager::saveAllClouds | ( | QString | filename, |
bool | threaded = true |
||
) | [slot] |
Call saveAllCloudsToFile, as background thread if threaded=true and possible.
Definition at line 192 of file graph_mgr_io.cpp.
void GraphManager::saveAllCloudsToFile | ( | QString | filename | ) | [protected] |
iterate over all Nodes, transform them to the fixed frame, aggregate and save
will hold all other clouds
will hold all other clouds
FIXME: Should transform to <fixed_frame_name>
Definition at line 501 of file graph_mgr_io.cpp.
void GraphManager::saveAllFeatures | ( | QString | filename, |
bool | threaded = true |
||
) | [slot] |
Call saveAllFeaturesToFile, as background thread if threaded=true and possible.
Definition at line 434 of file graph_mgr_io.cpp.
void GraphManager::saveAllFeaturesToFile | ( | QString | filename | ) | [protected] |
Transform all feature positions to global coordinates and save them together with the belonging descriptors.
Definition at line 444 of file graph_mgr_io.cpp.
void GraphManager::saveBagfile | ( | QString | filename | ) | [slot] |
iterate over all Nodes, writing their transform and pointcloud to a bagfile
Definition at line 101 of file graph_mgr_io.cpp.
void GraphManager::saveBagfileAsync | ( | QString | filename | ) | [slot] |
iterate over all Nodes, writing their transform and pointcloud to a bagfile. If parameter concurrent_io is set, do it in a background thread
Definition at line 92 of file graph_mgr_io.cpp.
void GraphManager::saveG2OGraph | ( | QString | filename | ) | [slot] |
Save graph in g2o format.
Definition at line 932 of file graph_mgr_io.cpp.
void GraphManager::saveIndividualClouds | ( | QString | file_basename, |
bool | threaded = true |
||
) | [slot] |
Call saveIndividualCloudsToFile, as background thread if threaded=true and possible.
Definition at line 202 of file graph_mgr_io.cpp.
void GraphManager::saveIndividualCloudsToFile | ( | QString | filename | ) | [protected] |
iterate over all Nodes, save each in one pcd-file
Definition at line 329 of file graph_mgr_io.cpp.
void GraphManager::saveOctomap | ( | QString | filename, |
bool | threaded = true |
||
) | [slot] |
Save the cloud as octomap by using the (internal) octomap server to render the point clouds into the voxel map.
Definition at line 236 of file graph_mgr_io.cpp.
void GraphManager::saveOctomapImpl | ( | QString | filename | ) | [protected] |
Definition at line 252 of file graph_mgr_io.cpp.
void GraphManager::savePlyFile | ( | QString | filename, |
pointcloud_normal_type & | full_cloud | ||
) | [protected] |
Definition at line 1161 of file graph_mgr_io.cpp.
void GraphManager::saveTrajectory | ( | QString | filebasename, |
bool | with_ground_truth = false |
||
) | [slot] |
Save trajectory of computed motion and ground truth (if available)
Definition at line 614 of file graph_mgr_io.cpp.
void GraphManager::sendAllClouds | ( | bool | threaded = true | ) | [slot] |
iterate over all Nodes, sending their transform and pointcloud
This file contains the methods of GraphManager concerned with transfering data via ROS, to the screen (visualization) or to disk. They are declared in graph_manager.h
Definition at line 45 of file graph_mgr_io.cpp.
void GraphManager::sendAllCloudsImpl | ( | ) | [protected] |
Definition at line 152 of file graph_mgr_io.cpp.
void GraphManager::sendFinished | ( | ) | [signal] |
Compute information matrix from empirical variances. If graph_filename is given, compute variances from
Definition at line 138 of file graph_manager2.cpp.
void GraphManager::setEmpiricalCovariancesForEdgeSet | ( | EdgeSet & | edges | ) | [protected] |
See setEmpiricalCovariances.
Definition at line 111 of file graph_manager2.cpp.
void GraphManager::setFeatures | ( | const std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > * | ) | [signal] |
void GraphManager::setGraph | ( | const g2o::OptimizableGraph * | ) | [signal] |
void GraphManager::setGraphEdges | ( | const QList< QPair< int, int > > * | edge_list | ) | [signal] |
void GraphManager::setGUIInfo | ( | QString | message | ) | [signal] |
void GraphManager::setGUIInfo2 | ( | QString | message | ) | [signal] |
void GraphManager::setGUIStatus | ( | QString | message | ) | [signal] |
void GraphManager::setOptimizerVerbose | ( | bool | verbose | ) |
Definition at line 104 of file graph_manager.cpp.
void GraphManager::setPointCloud | ( | pointcloud_type * | pc, |
QMatrix4x4 | transformation | ||
) | [signal] |
tf::StampedTransform GraphManager::stampedTransformInWorldFrame | ( | const Node * | node, |
const tf::Transform & | computed_motion | ||
) | const [protected] |
Compute the transform between the fixed frame (usually the initial position of the cam) and the node from the motion (given in sensor frame)
Definition at line 938 of file graph_mgr_io.cpp.
void GraphManager::toggleMapping | ( | bool | mappingOn | ) | [slot] |
This file contains methods of the GraphManager class without extra dependencies (except for the class header).
Definition at line 25 of file graph_manager2.cpp.
bool GraphManager::updateCloudOrigin | ( | Node * | node | ) | [protected] |
Write current position estimate to the node's point cloud's origin Make sure to acquire the optimizer_mutex_ before calling
Write current pose estimate to pcl cloud header Returns false if node has no valid estimate or is empty
Definition at line 215 of file graph_mgr_io.cpp.
void GraphManager::updateTransforms | ( | QList< QMatrix4x4 > * | transformations | ) | [signal] |
void GraphManager::visualizeFeatureFlow3D | ( | unsigned int | marker_id = 0 , |
bool | draw_outlier = true |
||
) | [protected] |
Send markers to visualize the last matched features in rviz (if somebody subscribed)
Definition at line 686 of file graph_mgr_io.cpp.
void GraphManager::visualizeGraphEdges | ( | ) | const [protected] |
Send markers to visualize the graph edges (cam transforms) in rviz (if somebody subscribed)
Definition at line 777 of file graph_mgr_io.cpp.
void GraphManager::visualizeGraphIds | ( | ) | const [protected] |
Send markers to visualize the graph ids in rviz (if somebody subscribed)
void GraphManager::visualizeGraphNodes | ( | ) | const [protected] |
Send markers to visualize the graph nodes (cam positions) in rviz (if somebody subscribed)
Definition at line 833 of file graph_mgr_io.cpp.
void GraphManager::writeOctomap | ( | QString | filename | ) | const |
Only write (not create, not clear) existing octomap.
Definition at line 311 of file graph_mgr_io.cpp.
ros::Publisher GraphManager::batch_cloud_pub_ [protected] |
Definition at line 324 of file graph_manager.h.
bool GraphManager::batch_processing_runs_ [protected] |
Definition at line 341 of file graph_manager.h.
tf::TransformBroadcaster GraphManager::br_ [mutable, protected] |
Used to broadcast the pose estimate.
Definition at line 330 of file graph_manager.h.
g2o::SparseOptimizer::Vertex* GraphManager::calibration_vertex_ [protected] |
Definition at line 384 of file graph_manager.h.
"Regular" edges from camera to camera as found form feature correspondeces
Definition at line 180 of file graph_manager.h.
g2o::HyperGraph::VertexSet GraphManager::camera_vertices |
Pose vertices (in camera coordinate system)
Definition at line 178 of file graph_manager.h.
ColorOctomapServer GraphManager::co_server_ [protected] |
Definition at line 357 of file graph_manager.h.
tf::Transform GraphManager::computed_motion_ [protected] |
transformation of the last frame to the first frame (assuming the first one is fixed)
Definition at line 331 of file graph_manager.h.
MatchingResult GraphManager::curr_best_result_ [protected] |
will contain the motion to the best matching node
Definition at line 229 of file graph_manager.h.
std::string GraphManager::current_backend_ [protected] |
Definition at line 355 of file graph_manager.h.
QList<QPair<int, int> > GraphManager::current_edges_ [protected] |
Definition at line 310 of file graph_manager.h.
Definition at line 186 of file graph_manager.h.
int GraphManager::earliest_loop_closure_node_ [protected] |
Definition at line 356 of file graph_manager.h.
std::map<int, Node* > GraphManager::graph_ [protected] |
Definition at line 338 of file graph_manager.h.
tf::Transform GraphManager::init_base_pose_ [protected] |
Definition at line 332 of file graph_manager.h.
QList<int> GraphManager::keyframe_ids_ [protected] |
Definition at line 349 of file graph_manager.h.
unsigned int GraphManager::last_odom_time_ [protected] |
Definition at line 389 of file graph_manager.h.
Definition at line 333 of file graph_manager.h.
bool GraphManager::localization_only_ [protected] |
Definition at line 343 of file graph_manager.h.
unsigned int GraphManager::loop_closures_edges [protected] |
Definition at line 352 of file graph_manager.h.
unsigned int GraphManager::marker_id_ [protected] |
Definition at line 340 of file graph_manager.h.
ros::Publisher GraphManager::marker_pub_ [protected] |
Definition at line 321 of file graph_manager.h.
unsigned int GraphManager::next_seq_id [protected] |
Definition at line 353 of file graph_manager.h.
unsigned int GraphManager::next_vertex_id [protected] |
Definition at line 354 of file graph_manager.h.
Edges added by addOdometryEdgeToG2O(...)
Contains those motions that represent absence of information. Only there to prevent the graph from becoming unconnected.
Definition at line 185 of file graph_manager.h.
ros::Publisher GraphManager::online_cloud_pub_ [protected] |
Definition at line 325 of file graph_manager.h.
QMutex GraphManager::optimization_mutex_ [protected] |
This mutex restricts the optimization computation itself.
Definition at line 347 of file graph_manager.h.
g2o::SparseOptimizer* GraphManager::optimizer_ [protected] |
Definition at line 319 of file graph_manager.h.
QMutex GraphManager::optimizer_mutex_ [protected] |
This mutex restricts access to the optimizer's data structures.
Definition at line 345 of file graph_manager.h.
bool GraphManager::process_node_runs_ [protected] |
Definition at line 342 of file graph_manager.h.
ros::Publisher GraphManager::ransac_marker_pub_ [protected] |
Definition at line 322 of file graph_manager.h.
bool GraphManager::reset_request_ [protected] |
Definition at line 339 of file graph_manager.h.
g2o::RobustKernelHuber GraphManager::robust_kernel_ [protected] |
Definition at line 382 of file graph_manager.h.
unsigned int GraphManager::sequential_edges [protected] |
Definition at line 352 of file graph_manager.h.
ros::Timer GraphManager::timer_ [protected] |
Used to start the broadcasting of the pose estimate regularly.
Definition at line 328 of file graph_manager.h.
ros::Publisher GraphManager::whole_cloud_pub_ [protected] |
Definition at line 323 of file graph_manager.h.