Public Slots | Signals | Public Member Functions | Public Attributes | Protected Types | Protected Member Functions | Protected Attributes
GraphManager Class Reference

Computes a globally optimal trajectory from transformations between Node-pairs. More...

#include <graph_manager.h>

List of all members.

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_

Detailed Description

Computes a globally optimal trajectory from transformations between Node-pairs.

Definition at line 67 of file graph_manager.h.


Member Typedef Documentation

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.


Constructor & Destructor Documentation

Definition at line 58 of file graph_manager.cpp.

Definition at line 144 of file graph_manager.cpp.


Member Function Documentation

bool GraphManager::addEdgeToG2O ( const LoadedEdge3D edge,
bool  good_edge,
bool  set_estimate = false 
) [protected]

Definition at line 924 of file graph_manager.cpp.

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.

Here is the caller graph for this function:

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.

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.

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.

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.


Member Data Documentation

Definition at line 206 of file graph_manager.h.

Definition at line 198 of file graph_manager.h.

Definition at line 219 of file graph_manager.h.

Used to broadcast the pose estimate.

Definition at line 203 of file graph_manager.h.

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.

Definition at line 205 of file graph_manager.h.

QList<int> GraphManager::keyframe_ids_ [protected]

Definition at line 225 of file graph_manager.h.

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.

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.

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.

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.

Definition at line 220 of file graph_manager.h.

Definition at line 196 of file graph_manager.h.

Definition at line 215 of file graph_manager.h.

unsigned int GraphManager::sequential_edges [protected]

Definition at line 228 of file graph_manager.h.

Definition at line 222 of file graph_manager.h.

Used to start the broadcasting of the pose estimate regularly.

Definition at line 201 of file graph_manager.h.

Definition at line 197 of file graph_manager.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


rgbdslam
Author(s): Felix Endres, Juergen Hess, Nikolas Engelhard
autogenerated on Wed Dec 26 2012 15:53:09