feature_viewer is an application available in ccny_rgbd which lets users test out different sparse feature detectors on top of RGBD data produced by rgbd_image_proc (or a compatible program). It allows users to change setting via dynamic_reconfigure and visualize features and their associated covariances in rviz.

feature_viewer is an auxiliary tool; it is not part of the visual odometry and mapping pipeline.

feature_viewer.png

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

feature/cloud (sensor_msgs/PointCloud2)

  • Point cloud of the detected features
feature/covariances (visualization_msgs/Marker)
  • Covariance matrices of the detected features. The 3x3 cov. matrix is visualized by the 3 principal components, each with a length of +/- 3 sigmas.

Parameters

General parameters

~queue_size (int, default: 5)

  • input and output queue sizes

Feature parameters

~feature/detector_type (string, default: "GFT")

  • Type of feature detector. Available types: "GFT", "SURF", "STAR", "ORB"
~feature/show_keypoints (bool, default: false)
  • Whether to show the detected keypoint image in an OpenCV window.
~feature/publish_cloud (bool, default: false)
  • Whether to publish the point cloud of 3D features
~feature/publish_covariances (bool, default: false)
  • Whether to calculate and publish the covariance matrix markers
~feature/smooth (int, default: 0)
  • Half-size of the window for the Gaussian smoothing kernel. 0 = no smoothing
~feature/max_range (double, default: 5.5)
  • Maximum z-range, in meters, allowed for valid features
~feature/GFT/n_features (int, default: 400)
  • (GFT detector only) Number of desired features
~feature/GFT/min_distance (int, default: 1)
  • (GFT detector only) Minimum distance, in pixels, between detected features
~feature/ORB/n_features (int, default: 400)
  • (ORB detector only) Number of desired features
~feature/ORB/threshold (double, default: 31.0)
  • (ORB detector only) Detection threshold
~feature/STAR/min_distance (int, default: 2)
  • (STAR detector only) Minimum distance, in pixels, between detected features
~feature/STAR/threshold (double, default: 32.0)
  • (STAR detector only) Detection threshold
~feature/SURF/threshold (double, default: 400.0)
  • (SURF detector only) Detection threshold

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 the feature_viewer.launch:

roslaunch ccny_rgbd feature_viewer.launch 

This will start feature_viewer, rviz, and dynamic_reconfigure. A config file is passed to rviz for convenience.

The video below shows an overview of the functionality. We recommend viewing in HD and big window to see text clearly.

Here's a breakdown of what we demonstrate in the video:

  • go to rgbd_image_proc, set publish_cluod to true

  • go to feature_viewer_node, enable visualizations

  • in feature_viewer_node, change detector type to "SURF"

  • go to feature_viewer_node/feature/SURF, change threshold parameter

Wiki: ccny_rgbd/feature_viewer (last edited 2013-02-12 20:25:06 by IvanDryanovski)