visual_odometry is an application available in ccny_rgbd for estimating the motion of a moving RGBD camera. 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.

visual_odometry is a real-time application with relatively low computational requirements. It does not output a map, and does not perform SLAM in the sense of closing large loops in the trajectory. For a graph-based SLAM solution, visual_odometry can be used in conjuntion with keyframe_mapper.

vo.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

odom (nav_msgs/Odometry)

  • Pose of the base frame in relation to the fixed frame, as an odometry message.
feature/cloud (sensor_msgs/PointCloud2)
  • Point cloud of the detected features
model/cloud (sensor_msgs/PointCloud2)
  • Point cloud of the sparse model maintained for registration.
model/covariances (visualization_msgs/Marker)
  • Covariance matrices of the sparse model 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
~base_frame (string, default: "camera_link")
  • The moving frame
~fixed_frame (string, default: "odom")
  • The fixed frame
~publish_tf (bool, default: true)
  • Whether to publish a tf transform between the fixed and moving frames.

Feature parameters

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

  • Type of feature detector. Available types: "GFT", "SURF", "STAR", "ORB"
~feature/publish_cloud (bool, default: false)
  • Whether to publish the point cloud of 3D features
~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

Registration parameters

~reg/reg_type (string, default: "ICPProbModel")

  • Registration type. Available options: "ICPProbModel", "ICP" (deprecated)
~reg/motion_constraint (int, default: 0)
  • Dimensions which are constrained (no motion allowed). NONE = 0, ROLL_PITCH = 1, ROLL_PITCH_Z = 2. A ground robot, for example, will usually have ROLL_PITCH_Z constraints.
~reg/ICPProbModel/max_iterations (int, default: 10)
  • Maximum ICP iterations convergence criteria for ICP
~reg/ICPProbModel/min_correspondences (int, default: 15)
  • Minimum required correspondences for ICP
~reg/ICPProbModel/max_model_size (int, default: 3000)
  • Maximum number of features in the sparse model. Once saturated, new points will overwrite old points.
~reg/ICPProbModel/max_corresp_dist_eucl (double, default: 0.15)
  • Maximum distance (in meters) for ICP correspondences
~reg/ICPProbModel/max_assoc_dist_mah (double, default: 10.0)
  • Maximum Mahalanobis distance (unitless) for feature-to-model association in Kalman Filter
~reg/ICPProbModel/n_nearest_neighbors (int, default: 4)
  • Number of Euclidean candidates to consider for finding best Mahalanobis nearest neighbor for Kalman Filter
~reg/ICPProbModel/publish_model_cloud (bool, default: false)
  • Whether to publish the sparse model as a point cloud
~reg/ICPProbModel/publish_model_covariance (bool, default: false)
  • Whether to publish the sparse model covariances as a visualization markers. Markers will display the 3 principal axes of the 3x3 covariance matrix, with a length of +/- 3 sigmas
~reg/ICPProbModel/tf_epsilon_linear (double, default: 1e-4)
  • Linear convergence criteria for ICP
~reg/ICPProbModel/tf_epsilon_angular (double, default: 1.7e-3)
  • Angular convergence criteria for ICP

Transforms

Required tf Transforms

camera_link[incoming image frame]
  • Transform from the moving frame to the optical camera frame.

Provided tf Transforms

odomcamera_link
  • The pose of the moving frame in the fixed frame. Only provided when publish_tf is enabled.

Services

save_sparse_map (ccny_rgbd/Save)

  • Saves the sparse feature model as a pcd point cloud.

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 visual_odometry.launch:

roslaunch ccny_rgbd visual_odometry.launch

Wiki: ccny_rgbd/visual_odometry (last edited 2013-02-06 18:12:18 by IvanDryanovski)