keyframe_mapper is an application available in ccny_rgbd for graph-based offline SLAM. It also servers as an online map server which can save, load, and visualize PointCloud maps. It requires a stream of registered depth and rgb images, as well as a CameraInfo matrix. These topics can be generated using rgbd_image_proc in conjunctions with ccny_openni_launch, or any compatible driver. It further requires the pose of the camera to be calculated in some fixed frame, which can be done via visual_odometry.

keyframe_mapper supports the following file input formats:

• Keyframes (raw rgb and depth images + metadata)

keyframe_mapper supports the following file output formats:

• Keyframes (raw rgb and depth images + metadata)

• PCL PointCloud (.pcd file)

• Octomap (.ot file, color supported)

Here are some examples of 3D maps in different formats (click for larger image).

ROS API

Subscribed Topics

/rgbd/rgb (sensor_msgs/Image)

• Input RGB image

/rgbd/depth (sensor_msgs/Image)

• Input Depth image (registered in the RGB frame)

/rgbd/info (sensor_msgs/CameraInfo)

• CameraInfo, applies to both images.

Published Topics

keyframes (sensor_msgs/PointCloud2)

• Dense PointClouds of stored RGBD keyframes.

keyframe_poses (visualization_msgs/Marker)

• Visualization marker of keyframe poses

keyframe_associations (visualization_msgs/Marker)

• Visualization marker of keyframe associations

Parameters

General parameters

~queue_size (int, default: 5)

• input and output queue sizes

~fixed_frame (string, default: "odom")

• The fixed frame

~pcd_map_res (double, default: 0.01)

• The map resolution (in meters) for saving the pointcloud map

~octomap_res (double, default: 0.05)

• The map resolution (in meters) for saving the octomap

~octomap_with_color (bool, default: true)

• Whether to save the octomap with color information per voxel.

~kf_dist_eps (double, default: 0.10)

• Linear threshold (in meters) of distance traveled since last keyframe. If threshold is crossed, a new keyframe will be inserted.

~kf_angle_eps (double, default: 0.174)

• Angular threshold (in radians) of distance traveled since last keyframe. If threshold is crossed, a new keyframe will be inserted.

Graph parameters

~graph/n_keypoints (int, default: 200)

• Number of desired keypoints to detect in each image for RANSAC processing

~graph/max_ransac_iterations (int, default: 2000)

• Maximum iterations for pairwise RANSAC test

~graph/n_ransac_candidates (int, default: 15)

• How many candidate keyframes will be tested against the query keyframe using a pairwise RANSAC test

~graph/k_nearest_neighbors (int, default: 15)

• How many nearest neighbors are requested per keypoint from the kd-tree of all keypoints

~graph/min_ransac_inliers (int, default: 30)

• How many inliers are required to pass the RANSAC test

~graph/max_corresp_dist_desc (double, default: 1.0)

• Maximum distance (in SURF descriptor space) between two features to be considered a correspondence candidate

~graph/max_corresp_dist_eucl (double, default: 0.03)

• Maximum distance (in Euclidean metric space) between two features to be considered a correspondence candidate.

~graph/save_ransac_results (bool, default: false)

• If true, positive RANSAC results will be saved to file as images with keypoint correspondences

~graph/ransac_results_path (string, default: $HOME)

• The path where to save images if save_ransac_results is true

Transforms

Required tf Transforms

camera_link → [incoming image frame]

• Transform from the moving frame to the optical camera frame.

Services

publish_keyframe (ccny_rgbd/PublishKeyframe)

• Publishes a single keyframe, specified by its id.

publish_keyframes (ccny_rgbd/PublishKeyframes)

• Publishes a set of keyframes. The argument should be a regular expression string matching the index of the desired keyframes to be published. Note: passing a string may require escaping the quotation marks, otherwise ROS sometimes confuses it for an integer. Example 1 (publishes all keyframes): /publish_keyframes ".*" Example 2 (publishes keyframes 1 to 9): /publish_keyframes \"[1-9]\"

save_keyframes (ccny_rgbd/Save)

• Save all the keyframes to disk. The argument should be a string with the directory where to save the keyframes.

load_keyframes (ccny_rgbd/Load)

• Load keyframes from disk. The argument should be a string with the directory pointing to the keyframes.

save_pcd_map (ccny_rgbd/Save)

• Create an aggregate map from all the keyframes and save it as a .pcd file to disk. The argument should be the path to the .pcd file.

save_octomap (ccny_rgbd/Save)

• Create an aggregate map from all the keyframes and save it as a octomap file to disk. The argument should be the path to the .ot file.

add_manual_keyframe (ccny_rgbd/AddManualKeyframe)

• Request a keyframe to be immediately added manually, bypassing the distance-traveled thresholds.

generate_graph (ccny_rgbd/GenerateGraph)

• Generate the correspondence graph between all the keyframes.

solve_graph (ccny_rgbd/SolveGraph)

• Perform global alignement on the keyframe graph, changing the pose of the keyframes. Note: generate_graph must be called prior to this.

Usage

First, launch the device and image processing via ccny_openni_launch.

Note: You can use any launcher, as long as it outputs a registered depth and rgb image, and the corresponding CameraInfo (see API above).

roslaunch ccny_openni_launch openni.launch 

Next, launch vo+mapping.launch, which starts the visual odometry and mapping interfaces.

roslaunch ccny_rgbd vo+mapping.launch

You can visualize the current list of keyframes using rviz

rosrun rviz rviz
rosservice call /publish_keyframes ".*"

Once you have finished gathering data, call the services to generate the graph and solve it:

rosservice call /generate_graph
rosservice call /solve_graph

Once that is done, you can republish the frames to rviz, or save them to disk, for example:

rosservice call /save_keyframes "$HOME/ros/my_keyframe_sequence"

Or you can save the entire map as a pcd file:

rosservice call /save_pcd_map "$HOME/ros/my_map.pcd"

Once you have the keyframes saved, you can load them in the keyframe_mapper without running the rest of the system. For example:

roscore
rosrun ccny_rgbd keyframe_mapper_node
rosservice call /load_keyframes "$HOME/ros/my_keyframe_sequence"
rosservice call /publish_keyframes ".*"
etc