Computes a globally optimal trajectory from transformations between Node-pairs. More...
#include <graph_manager.h>
Public Slots | |
| void | cloudRendered (pointcloud_type const *pc) |
| void | deleteLastFrame () |
| Throw the last node out, reoptimize. | |
| void | optimizeGraph (int iter=-1, bool nonthreaded=false) |
| Calls optimizeGraphImpl either in fore- or background depending on parameter concurrent_optimization. | |
| void | printEdgeErrors (QString) |
| void | pruneEdgesWithErrorAbove (float) |
| 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 | saveIndividualClouds (QString file_basename, bool threaded=true) |
| Call saveIndividualCloudsToFile, as background thread if threaded=true and possible. | |
| 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 | setMaxDepth (float max_depth) |
| void | toggleMapping (bool) |
Signals | |
| void | deleteLastNode () |
| void | newTransformationMatrix (QString) |
| Connect to this signal to get the transformation matrix from the last frame as QString. | |
| void | resetGLViewer () |
| void | sendFinished () |
| void | setGraphEdges (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 | 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 | finishUp () |
| void | firstNode (Node *new_node) |
| Adds the first node. | |
| GraphManager (ros::NodeHandle) | |
| bool | isBusy () |
| ~GraphManager () | |
Public Attributes | |
| float | Max_Depth |
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, bool good_edge, bool set_estimate=false) |
| void | broadcastTransform (Node *node, tf::Transform &computed_motion) |
| void | createOptimizer (std::string backend, g2o::SparseOptimizer *optimizer=NULL) |
| Instanciate the optimizer with the desired backend. | |
| double | geodesicDiscount (g2o::HyperDijkstra &hypdij, const MatchingResult &mr) |
| QList< QMatrix4x4 > * | getAllPosesAsMatrixList () |
| 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 > | getPotentialEdgeTargets (const Node *new_node, int sequential_targets, int sample_targets=5) |
| QList< int > | getPotentialEdgeTargetsWithDijkstra (const Node *new_node, int sequential_targets, int geodesic_targets, int sampled_targets=5, int predecessor_id=-1) |
| void | mergeAllClouds (pointcloud_type &merge) |
| void | optimizeGraphImpl (int max_iter) |
| Computes optimization. | |
| void | pointCloud2MeshFile (QString filename, pointcloud_type full_cloud) |
| void | resetGraph () |
| 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 | sendAllCloudsImpl () |
| void | visualizeFeatureFlow3D (unsigned int marker_id=0, bool draw_outlier=true) const |
| 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 | |
| tf::Transform | base2points_ |
| ros::Publisher | batch_cloud_pub_ |
| bool | batch_processing_runs_ |
| tf::TransformBroadcaster | br_ |
| Used to broadcast the pose estimate. | |
| tf::Transform | computed_motion_ |
| transformation of the last frame to the first frame (assuming the first one is fixed) | |
| std::string | current_backend_ |
| QList< QPair< int, int > > | current_edges_ |
| QList< QMatrix4x4 > | current_poses_ |
| g2o::SE3Quat | edge_to_previous_node_ |
| std::map< int, Node * > | graph_ |
| tf::Transform | init_base_pose_ |
| QList< int > | keyframe_ids_ |
| std::vector< cv::DMatch > | last_inlier_matches_ |
| std::vector< cv::DMatch > | last_matches_ |
| int | last_matching_node_ |
| QMatrix4x4 | latest_transform_ |
| same as computed_motion_ as Eigen | |
| bool | localization_only_ |
| unsigned int | loop_closures_edges |
| unsigned int | marker_id_ |
| ros::Publisher | marker_pub_ |
| g2o::SparseOptimizer * | optimizer_ |
| QMutex | optimizer_mutex |
| bool | process_node_runs_ |
| ros::Publisher | ransac_marker_pub_ |
| bool | reset_request_ |
| unsigned int | sequential_edges |
| bool | someone_is_waiting_for_me_ |
| 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 67 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 212 of file graph_manager.h.
Definition at line 58 of file graph_manager.cpp.
Definition at line 144 of file graph_manager.cpp.
| bool GraphManager::addEdgeToG2O | ( | const LoadedEdge3D & | edge, |
| bool | good_edge, | ||
| bool | set_estimate = false |
||
| ) | [protected] |
Definition at line 924 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
Definition at line 417 of file graph_manager.cpp.
| void GraphManager::broadcastTransform | ( | Node * | node, |
| tf::Transform & | computed_motion | ||
| ) | [protected] |
Send the transform between openni_camera (the initial position of the cam) and the cumulative motion.
Definition at line 1423 of file graph_manager.cpp.
| void GraphManager::cloudRendered | ( | pointcloud_type const * | pc | ) | [slot] |
Definition at line 1511 of file graph_manager.cpp.
| void GraphManager::createOptimizer | ( | std::string | backend, |
| g2o::SparseOptimizer * | optimizer = NULL |
||
| ) | [protected] |
Instanciate the optimizer with the desired backend.
Definition at line 93 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 136 of file graph_manager.cpp.
| void GraphManager::deleteLastFrame | ( | ) | [slot] |
Throw the last node out, reoptimize.
Definition at line 1067 of file graph_manager.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 160 of file graph_manager.cpp.
| void GraphManager::finishUp | ( | ) |
Definition at line 1560 of file graph_manager.cpp.
| void GraphManager::firstNode | ( | Node * | new_node | ) |
Adds the first node.
Definition at line 389 of file graph_manager.cpp.
| double GraphManager::geodesicDiscount | ( | g2o::HyperDijkstra & | hypdij, |
| const MatchingResult & | mr | ||
| ) | [protected] |
Definition at line 1567 of file graph_manager.cpp.
| QList< QMatrix4x4 > * GraphManager::getAllPosesAsMatrixList | ( | ) | [protected] |
Return pointer to a list of the optimizers graph poses on the heap(!)
Constructs a list of transformation matrices from all nodes (used for visualization in glviewer) The object lives on the heap and needs to be destroyed somewhere else (i.e. in glviewer)
Definition at line 1117 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 1099 of file graph_manager.cpp.
| QList< int > GraphManager::getPotentialEdgeTargets | ( | const Node * | new_node, |
| int | sequential_targets, | ||
| int | sample_targets = 5 |
||
| ) | [protected] |
Suggest nodes for comparison. <sequential_targets> direct predecessors in the time sequence and <sample_targets> randomly chosen from the remaining.
Definition at line 321 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 |
||
| ) | [protected] |
Similar to getPotentialEdgeTargets(...), but instead of looking for temporal predecessors it looks for the nearest neighbours in the graph structure. This 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 227 of file graph_manager.cpp.
Definition at line 1563 of file graph_manager.cpp.
| void GraphManager::mergeAllClouds | ( | pointcloud_type & | merge | ) | [protected] |
| void GraphManager::newTransformationMatrix | ( | QString | ) | [signal] |
Connect to this signal to get the transformation matrix from the last frame as QString.
| void GraphManager::optimizeGraph | ( | int | iter = -1, |
| bool | nonthreaded = false |
||
| ) | [slot] |
Calls optimizeGraphImpl either in fore- or background depending on parameter concurrent_optimization.
Definition at line 981 of file graph_manager.cpp.
| void GraphManager::optimizeGraphImpl | ( | int | max_iter | ) | [protected] |
Computes optimization.
Definition at line 991 of file graph_manager.cpp.
| void GraphManager::pointCloud2MeshFile | ( | QString | filename, |
| pointcloud_type | full_cloud | ||
| ) | [protected] |
Definition at line 1334 of file graph_manager.cpp.
| void GraphManager::printEdgeErrors | ( | QString | filename | ) | [slot] |
Definition at line 1545 of file graph_manager.cpp.
| void GraphManager::pruneEdgesWithErrorAbove | ( | float | thresh | ) | [slot] |
Definition at line 1534 of file graph_manager.cpp.
| void GraphManager::reset | ( | ) | [slot] |
Start over with new graph.
Publish the updated transforms for the graph node resp. clouds
void GraphManager::publishCorrectedTransforms(){ struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); fill message rgbdslam::CloudTransforms msg; for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) { g2o::VertexSE3* v = optimizer_->vertex(i); tf::Transform trans = g2o2TF(v->estimate()); geometry_msgs::Transform trans_msg; tf::transformTFToMsg(trans,trans_msg); msg.transforms.push_back(trans_msg); msg.ids.push_back(graph_[i]->msg_id_); //msg_id is no more } msg.header.stamp = ros::Time::now();
if (transform_pub_.getNumSubscribers() > 0) transform_pub_.publish(msg); clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); }
Definition at line 1063 of file graph_manager.cpp.
| void GraphManager::resetGLViewer | ( | ) | [signal] |
| void GraphManager::resetGraph | ( | ) | [protected] |
Definition at line 362 of file graph_manager.cpp.
| void GraphManager::sanityCheck | ( | float | thresh | ) | [slot] |
Definition at line 1522 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 1138 of file graph_manager.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
Definition at line 1285 of file graph_manager.cpp.
| void GraphManager::saveAllFeatures | ( | QString | filename, |
| bool | threaded = true |
||
| ) | [slot] |
Call saveAllFeaturesToFile, as background thread if threaded=true and possible.
Definition at line 1226 of file graph_manager.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 1236 of file graph_manager.cpp.
| void GraphManager::saveIndividualClouds | ( | QString | file_basename, |
| bool | threaded = true |
||
| ) | [slot] |
Call saveIndividualCloudsToFile, as background thread if threaded=true and possible.
Definition at line 1148 of file graph_manager.cpp.
| void GraphManager::saveIndividualCloudsToFile | ( | QString | filename | ) | [protected] |
iterate over all Nodes, save each in one pcd-file
Definition at line 1158 of file graph_manager.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 1366 of file graph_manager.cpp.
| void GraphManager::sendAllClouds | ( | bool | threaded_if_available = true | ) | [slot] |
iterate over all Nodes, sending their transform and pointcloud
Definition at line 1446 of file graph_manager.cpp.
| void GraphManager::sendAllCloudsImpl | ( | ) | [protected] |
Definition at line 1457 of file graph_manager.cpp.
| void GraphManager::sendFinished | ( | ) | [signal] |
| void GraphManager::setGraphEdges | ( | 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::setMaxDepth | ( | float | max_depth | ) | [slot] |
Definition at line 1506 of file graph_manager.cpp.
| void GraphManager::setPointCloud | ( | pointcloud_type * | pc, |
| QMatrix4x4 | transformation | ||
| ) | [signal] |
| void GraphManager::toggleMapping | ( | bool | mappingOn | ) | [slot] |
Definition at line 1577 of file graph_manager.cpp.
| void GraphManager::updateTransforms | ( | QList< QMatrix4x4 > * | transformations | ) | [signal] |
| void GraphManager::visualizeFeatureFlow3D | ( | unsigned int | marker_id = 0, |
| bool | draw_outlier = true |
||
| ) | const [protected] |
Send markers to visualize the last matched features in rviz (if somebody subscribed)
Definition at line 682 of file graph_manager.cpp.
| void GraphManager::visualizeGraphEdges | ( | ) | const [protected] |
Send markers to visualize the graph edges (cam transforms) in rviz (if somebody subscribed)
Definition at line 771 of file graph_manager.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 827 of file graph_manager.cpp.
tf::Transform GraphManager::base2points_ [protected] |
Definition at line 206 of file graph_manager.h.
ros::Publisher GraphManager::batch_cloud_pub_ [protected] |
Definition at line 198 of file graph_manager.h.
bool GraphManager::batch_processing_runs_ [protected] |
Definition at line 219 of file graph_manager.h.
tf::TransformBroadcaster GraphManager::br_ [protected] |
Used to broadcast the pose estimate.
Definition at line 203 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 204 of file graph_manager.h.
std::string GraphManager::current_backend_ [protected] |
Definition at line 229 of file graph_manager.h.
QList<QPair<int, int> > GraphManager::current_edges_ [protected] |
Definition at line 187 of file graph_manager.h.
QList<QMatrix4x4> GraphManager::current_poses_ [protected] |
Definition at line 184 of file graph_manager.h.
g2o::SE3Quat GraphManager::edge_to_previous_node_ [protected] |
Definition at line 218 of file graph_manager.h.
std::map<int, Node* > GraphManager::graph_ [protected] |
Definition at line 214 of file graph_manager.h.
tf::Transform GraphManager::init_base_pose_ [protected] |
Definition at line 205 of file graph_manager.h.
QList<int> GraphManager::keyframe_ids_ [protected] |
Definition at line 225 of file graph_manager.h.
std::vector< cv::DMatch > GraphManager::last_inlier_matches_ [protected] |
Definition at line 146 of file graph_manager.h.
std::vector< cv::DMatch > GraphManager::last_matches_ [protected] |
Definition at line 147 of file graph_manager.h.
int GraphManager::last_matching_node_ [protected] |
Definition at line 217 of file graph_manager.h.
QMatrix4x4 GraphManager::latest_transform_ [protected] |
same as computed_motion_ as Eigen
Definition at line 208 of file graph_manager.h.
bool GraphManager::localization_only_ [protected] |
Definition at line 221 of file graph_manager.h.
unsigned int GraphManager::loop_closures_edges [protected] |
Definition at line 228 of file graph_manager.h.
unsigned int GraphManager::marker_id_ [protected] |
Definition at line 216 of file graph_manager.h.
ros::Publisher GraphManager::marker_pub_ [protected] |
Definition at line 195 of file graph_manager.h.
Definition at line 128 of file graph_manager.h.
g2o::SparseOptimizer* GraphManager::optimizer_ [protected] |
Definition at line 193 of file graph_manager.h.
QMutex GraphManager::optimizer_mutex [protected] |
Definition at line 223 of file graph_manager.h.
bool GraphManager::process_node_runs_ [protected] |
Definition at line 220 of file graph_manager.h.
ros::Publisher GraphManager::ransac_marker_pub_ [protected] |
Definition at line 196 of file graph_manager.h.
bool GraphManager::reset_request_ [protected] |
Definition at line 215 of file graph_manager.h.
unsigned int GraphManager::sequential_edges [protected] |
Definition at line 228 of file graph_manager.h.
bool GraphManager::someone_is_waiting_for_me_ [protected] |
Definition at line 222 of file graph_manager.h.
ros::Timer GraphManager::timer_ [protected] |
Used to start the broadcasting of the pose estimate regularly.
Definition at line 201 of file graph_manager.h.
ros::Publisher GraphManager::whole_cloud_pub_ [protected] |
Definition at line 197 of file graph_manager.h.
