<<TOC(4)>>

## AUTOGENERATED DON'T DELETE
## CategoryPackage

'''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 [[ccny_rgbd/rgbd_image_proc | 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 [[ccny_rgbd/visual_odometry | 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).

[[attachment:3dmap.png|{{attachment:3dmap.png|Pcd Map|width=350}}]]
[[attachment:3rooms_octomap.png|{{attachment:3rooms_octomap.png|Octomap, without color|width=350}}]]

[[attachment:3rooms_octomap_2.png|{{attachment:3rooms_octomap_2.png|Octomap, with color|width=350}}]]
[[attachment:room_octomap.png|{{attachment:room_octomap.png|Octomap, with color|width=350}}]]

== ROS API ==

=== Subscribed Topics ===
{{{
#!clearsilver CS/NodeAPI
sub {
no_header=True
   0{
    name = /rgbd/rgb
    type = sensor_msgs/Image
    desc = Input RGB image
    }
   1{
    name = /rgbd/depth
    type = sensor_msgs/Image
    desc = Input Depth image (registered in the RGB frame)
    }
   2{
    name = /rgbd/info
    type = sensor_msgs/CameraInfo
    desc = !CameraInfo, applies to both images.
    }
}
}}}
=== Published Topics ===
{{{
#!clearsilver CS/NodeAPI
pub {
no_header=True
   0{
    name = keyframes
    type = sensor_msgs/PointCloud2
    desc = Dense !PointClouds of stored RGBD keyframes.
    }
   1{
    name = keyframe_poses
    type = visualization_msgs/Marker
    desc = Visualization marker of keyframe poses
    }
   2{
    name = keyframe_associations
    type = visualization_msgs/Marker
    desc = Visualization marker of keyframe associations
    }
}
}}}

=== Parameters ===

==== General parameters ====
{{{
#!clearsilver CS/NodeAPI
param{
no_header=True
    0.name = ~queue_size
    0.type = int
    0.default = 5
    0.desc = input and output queue sizes

    1.name = ~fixed_frame
    1.type = string
    1.default = `"odom"`
    1.desc = The fixed frame

    2.name = ~pcd_map_res
    2.type = double
    2.default = 0.01
    2.desc = The map resolution (in meters) for saving the pointcloud map

    3.name = ~octomap_res
    3.type = double
    3.default = 0.05
    3.desc = The map resolution (in meters) for saving the octomap

    4.name = ~octomap_with_color
    4.type = bool
    4.default = true
    4.desc = Whether to save the octomap with color information per voxel.

    5.name = ~kf_dist_eps
    5.type = double
    5.default = 0.10
    5.desc = Linear threshold (in meters) of distance traveled since last keyframe. If threshold is crossed, a new keyframe will be inserted.

    6.name = ~kf_angle_eps
    6.type = double
    6.default = 0.174
    6.desc = Angular threshold (in radians) of distance traveled since last keyframe. If threshold is crossed, a new keyframe will be inserted.
}
}}}

==== Graph parameters ====
{{{
#!clearsilver CS/NodeAPI
param{
no_header=True

    5.name = ~graph/n_keypoints
    5.type = int
    5.default = 200
    5.desc = Number of desired keypoints to detect in each image for RANSAC processing

    6.name = ~graph/max_ransac_iterations
    6.type = int
    6.default = 2000
    6.desc = Maximum iterations for pairwise RANSAC test

    7.name = ~graph/n_ransac_candidates
    7.type = int
    7.default = 15
    7.desc = How many candidate keyframes will be tested against the query keyframe using a pairwise RANSAC test

    8.name = ~graph/k_nearest_neighbors
    8.type = int
    8.default = 15
    8.desc = How many nearest neighbors are requested per keypoint from the kd-tree of all keypoints

    9.name = ~graph/min_ransac_inliers
    9.type = int
    9.default = 30
    9.desc = How many inliers are required to pass the RANSAC test

    10.name = ~graph/max_corresp_dist_desc
    10.type = double
    10.default = 1.0
    10.desc = Maximum distance (in SURF descriptor space) between two features to be considered a correspondence candidate

    11.name = ~graph/max_corresp_dist_eucl
    11.type = double
    11.default = 0.03
    11.desc = Maximum distance (in Euclidean metric space) between two features to be considered a correspondence candidate.

    12.name = ~graph/save_ransac_results
    12.type = bool
    12.default = false
    12.desc =  If true, positive RANSAC results will be saved to file as images with keypoint correspondences

    13.name = ~graph/ransac_results_path
    13.type = string
    13.default = `$HOME`
    13.desc =  The path where to save images if `save_ransac_results` is true
}
}}}

=== Transforms ===

{{{
#!clearsilver CS/NodeAPI
no_header=True
 req_tf{
  0{
    from = camera_link
    to = [incoming image frame]
    desc = Transform from the moving frame to the optical camera frame.
  }
 }
}}}

=== Services ===
{{{
#!clearsilver CS/NodeAPI
srv{
no_header=True
  0.name = publish_keyframe
  0.type = ccny_rgbd/PublishKeyframe
  0.desc = Publishes a single keyframe, specified by its id.

  1.name = publish_keyframes
  1.type = ccny_rgbd/PublishKeyframes
  1.desc = 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]\" 

  2.name = save_keyframes
  2.type = ccny_rgbd/Save
  2.desc = Save all the keyframes to disk. The argument should be a string with the directory where to save the keyframes.

  3.name = load_keyframes
  3.type = ccny_rgbd/Load
  3.desc = Load keyframes from disk. The argument should be a string with the directory pointing to the keyframes.

  4.name = save_pcd_map
  4.type = ccny_rgbd/Save
  4.desc = 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.

  5.name = save_octomap
  5.type = ccny_rgbd/Save
  5.desc = 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.

  6.name = add_manual_keyframe
  6.type = ccny_rgbd/AddManualKeyframe
  6.desc = Request a keyframe to be immediately added manually, bypassing the distance-traveled thresholds.

  7.name = generate_graph
  7.type = ccny_rgbd/GenerateGraph
  7.desc = Generate the correspondence graph between all the keyframes.

  8.name = solve_graph
  8.type = ccny_rgbd/SolveGraph
  8.desc = 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 [[https://github.com/ccny-ros-pkg/ccny_rgbd_tools/blob/master/ccny_rgbd/launch/vo+mapping.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
}}}