Public Member Functions |
| void | addEdge (int i, int j, tf::Transform edge, int sigma) |
| | Add an edge to the graph.
|
| void | addFrameToQueue (Frame frame) |
| | Add a frame to the queue of frames to be inserted into the graph as vertices.
|
| void | findClosestVertices (int vertex_id, int window_center, int window, int best_n, vector< int > &neighbors) |
| | Get the closest neighbors by distance.
|
| cv::Mat | getCameraMatrix () const |
| | Get camera matrix.
|
| image_geometry::PinholeCameraModel | getCameraModel () const |
| | Get camera model.
|
| int | getFrameNum () const |
| | Get camera matrix.
|
| bool | getFramePose (int frame_id, tf::Transform &frame_pose) |
| | Get frame pose.
|
| void | getFrameVertices (int frame_id, vector< int > &vertices) |
| | Retrieve the list of the vertices of a corresponding frame.
|
| int | getLastVertexFrameId () |
| | Get the last vertex frame id.
|
| tf::Transform | getVertexCameraPose (int id, bool lock=true) |
| | Get the camera pose of some specific vertex.
|
| int | getVertexFrameId (int id) |
| | Get the frame id of some specific vertex.
|
| tf::Transform | getVertexPose (int vertex_id, bool lock=true) |
| | Get the graph vertex pose.
|
| tf::Transform | getVertexPoseRelativeToCamera (int id) |
| | Get the graph vertex pose relative to camera.
|
| | Graph (LoopClosing *loop_closing) |
| | Class constructor.
|
| void | init () |
| | Initialize the graph.
|
| void | run () |
| | Starts graph.
|
| void | saveGraph () |
| | Save the graph to file.
|
| void | setCamera2Odom (const tf::Transform &camera2odom) |
| | Set the transformation between camera and robot odometry frame.
|
| void | setCameraMatrix (const cv::Mat &camera_matrix) |
| | Set camera matrix.
|
| void | setCameraModel (const image_geometry::PinholeCameraModel &camera_model) |
| | Set camera model.
|
| void | update () |
| | Optimize the graph.
|
Protected Member Functions |
| int | addVertex (tf::Transform pose) |
| | Add a vertex to the graph.
|
| bool | checkNewFrameInQueue () |
| | Check if there are frames in the queue to be inserted into the graph.
|
| tf::Transform | correctClusterPose (tf::Transform initial_pose) |
| | Correct a cluster pose with the information of the updated graph.
|
| vector< vector< int > > | createComb (vector< int > cluster_ids) |
| | Return all possible combinations of 2 elements of the input vector.
|
| void | processNewFrame () |
| | Converts the frame to a graph vertex and adds it to the graph.
|
| void | publishCameraPose (tf::Transform camera_pose) |
| | Publishes the graph camera pose.
|
| void | publishGraph () |
| | Publishes all the graph.
|
| void | saveFrame (Frame frame, bool draw_clusters=false) |
| | Save the frame to the default location.
|
Private Attributes |
| tf::Transform | camera2odom_ |
| | > Mutex for the insertion of new frames into the graph
|
| cv::Mat | camera_matrix_ |
| | > Loop closing
|
| image_geometry::PinholeCameraModel | camera_model_ |
| | > The camera matrix
|
| vector< pair< int, int > > | cluster_frame_relation_ |
| | > Processed frames counter
|
| int | frame_id_ |
| | > Frames queue to be inserted into the graph
|
| list< Frame > | frame_queue_ |
| | > G2O graph optimizer
|
| vector< double > | frame_stamps_ |
| | > Stores the initial cluster poses, before graph update.
|
| g2o::SparseOptimizer | graph_optimizer_ |
| ros::Publisher | graph_pub_ |
| | > Camera pose publisher
|
| vector< tf::Transform > | initial_cluster_pose_history_ |
| | > Stores the cluster poses relative to camera frame
|
| vector< tf::Transform > | local_cluster_poses_ |
| | > Stores the cluster/frame relation (cluster_id, frame_id)
|
| LoopClosing * | loop_closing_ |
| | > Transformation between camera and robot odometry frame
|
| mutex | mutex_frame_queue_ |
| | > Mutex for the graph manipulation
|
| mutex | mutex_graph_ |
| ros::Publisher | pose_pub_ |
| | > Pinhole left camera model
|
Definition at line 40 of file graph.h.