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).
- Input RGB image
- Input Depth image (registered in the RGB frame)
- CameraInfo, applies to both images.
- Dense PointClouds of stored RGBD keyframes.
- Visualization marker of keyframe poses
- Visualization marker of keyframe associations
~queue_size (int, default: 5)
- input and output queue sizes
- The fixed frame
- The map resolution (in meters) for saving the pointcloud map
- The map resolution (in meters) for saving the octomap
- Whether to save the octomap with color information per voxel.
- Linear threshold (in meters) of distance traveled since last keyframe. If threshold is crossed, a new keyframe will be inserted.
- Angular threshold (in radians) of distance traveled since last keyframe. If threshold is crossed, a new keyframe will be inserted.
~graph/n_keypoints (int, default: 200)
- Number of desired keypoints to detect in each image for RANSAC processing
- Maximum iterations for pairwise RANSAC test
- How many candidate keyframes will be tested against the query keyframe using a pairwise RANSAC test
- How many nearest neighbors are requested per keypoint from the kd-tree of all keypoints
- How many inliers are required to pass the RANSAC test
- Maximum distance (in SURF descriptor space) between two features to be considered a correspondence candidate
- Maximum distance (in Euclidean metric space) between two features to be considered a correspondence candidate.
- If true, positive RANSAC results will be saved to file as images with keypoint correspondences
- The path where to save images if save_ransac_results is true
Required tf Transformscamera_link → [incoming image frame]
- Transform from the moving frame to the optical camera frame.
- Publishes a single keyframe, specified by its id.
- 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 all the keyframes to disk. The argument should be a string with the directory where to save the keyframes.
- Load keyframes from disk. The argument should be a string with the directory pointing to the keyframes.
- 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.
- 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.
- Request a keyframe to be immediately added manually, bypassing the distance-traveled thresholds.
- Generate the correspondence graph between all the keyframes.
- Perform global alignement on the keyframe graph, changing the pose of the keyframes. Note: generate_graph must be called prior to this.
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