Only released in EOL distros:   fuerte groovy hydro indigo

Overview

This package contains two nodes that talk to libviso2 (which is included in the libviso2 package): mono_odometer and stereo_odometer. Both estimate camera motion based on incoming rectified images from calibrated cameras. To estimate the scale of the motion, the mono odometer uses the ground plane and therefore needs information about the camera's z-coordinate and its pitch. The stereo odometer needs no additional parameters and works - if provided with images of good quality - out of the box.

The video below shows an online 3D reconstruction of a 3D scene shot by a Micro AUV using dense stereo point clouds coming from stereo_image_proc concatenated in rviz using the stereo odometer of this package.

In the repository, you can find a sample launch file, which uses a public bagfile available here: http://srv.uib.es/public/viso2_ros/sample_bagfiles/.

Used tfs

Please read REP 105 for an explanation of odometry frame ids.

The chain of transforms relevant for visual odometry is as follows:

• world → odom → base_link → camera

Visual odometry algorithms generally calculate camera motion. To be able to calculate robot motion based on camera motion, the transformation from the camera frame to the robot frame has to be known. Therefore this implementation needs to know the tf base_link → camera to be able to publish odom → base_link.

The name of the camera frame is taken from the incoming images, so be sure your camera driver publishes it correctly. If your camera driver does not set frame ids, you can use the fallback parameter sensor_frame_id (see below).

NOTE: The coordinate frame of the camera is expected to be the optical frame, which means x is pointing right, y downwards and z from the camera into the scene. The origin is where the camera's principle axis hits the image plane (as given in sensor_msgs/CameraInfo).

To learn how to publish the required tf base_link → camera, please refer to the tf tutorials. If the required tf is not available, the odometer assumes it as the identity matrix which means the robot frame and the camera frame are identical.

Limitations

libviso2 was designed to estimate the motion of a car using wide angle cameras. Cameras with large focal lengths have less overlap between consecutive images, especially on rotations and are therefore not recommended.

Monocular Odometry

In general, monocular odometry and SLAM systems cannot estimate motion or position on a metric scale. All estimates are relative to some unknown scaling factor. libviso2 overcomes this by assuming a fixed transformation from the ground plane to the camera (parameters camera_height and camera_pitch). To introduce these values, in each iteration the ground plane has to be estimated. That is why features on the ground as well as features above the ground are mandatory for the mono odometer to work.

Roughly the steps are the following:

1. Find F matrix from point correspondences using RANSAC and 8-point algorithm
2. Compute E matrix using the camera calibration
3. Compute 3D points and R|t up to scale
4. Estimate the ground plane in the 3D points

5. Use camera_height and camera_pitch to scale points and R|t

Unfortunately libviso2 does not provide sufficient introspection to signal if one of these steps fails.

Another problem occurs when the camera performs just pure rotation: even if there are enough features, the linear system to calculate the F matrix degenerates.

Stereo Odometry

In a properly calibrated stereo system 3D points can be calculated from a single image pair. The linear system to calculate camera motion is therefore based on 3D-3D point correspondences. There are no limitations for the camera movement or the feature distribution.

Nodes

Common for mono_odometer and stereo_odometer

Published Topics

~pose (geometry_msgs/PoseStamped)

• The robot's current pose according to the odometer.

~odometry (nav_msgs/Odometry)

• Odometry information that was calculated, contains pose, twist and covariances. NOTE: pose and twist covariance is not published yet.

~info (viso2_ros/VisoInfo)

• Message containing internal information on the libviso2 process regarding the current iteration.

Parameters

tf related

~odom_frame_id (string, default: /odom)

• Name of the world-fixed frame where the odometer lives.

~base_link_frame_id (string, default: /base_link)

• Name of the moving frame whose pose the odometer should report.

~publish_tf (bool, default: true)

• If true, the odometer publishes tf's (see above).

~sensor_frame_id (string, default: "/camera")

• Fallback sensor frame id. If the incoming camera info topic does not carry a frame id, this frame id will be used.

Bucketing parameters

~max_features (int, default: 2)

• Maximum number of features per bucket.

~bucket_width (double, default: 50.0)

• Width of the bucket.

~bucket_height (double, default: 50.0)

• Height of the bucket.

Matcher parameters

~nms_n (int, default: 3)

• Minimum distance between maxima in pixels for non-maxima-suppression.

~nms_tau (int, default: 50)

• Interest point peakiness threshold.

~match_binsize (int, default: 50)

• Matching width/height (affects efficiency only)

~match_radius (int, default: 200)

• Matching radius (du/dv in pixels)

~match_disp_tolerance (int, default: 2)

• dv tolerance for stereo matches (in pixels).

~outlier_disp_tolerance (int, default: 5)

• Disparity tolerance for outlier removal (in pixels).

~outlier_flow_tolerance (int, default: 5)

• Flow tolerance for outlier removal (in pixels).

~multi_stage (int, default: 1)

• 0=disabled, 1=multistage matching (denser and faster).

~half_resolution (int, default: 1)

• 0=disabled, 1=match at half resolution, refine at full resolution.

~refinement (int, default: 1)

• 0=none, 1=pixel, 2=subpixel.

Required tf Transforms

~base_link_frame_id → <frame_id attached to image messages>

• Transformation from the robot's reference point (base_link in most cases) to the camera's optical frame.

Provided tf Transforms

~odom_frame_id → ~base_link_frame_id

• Transformation from the odometry's origin (e.g. odom) to the robot's reference point (e.g. base_link)

mono_odometer

Subscribed Topics

image (sensor_msgs/Image)

• The rectified input image. There must be a corresponding camera_info topic as well. Internally, image_transport::CameraSubscriber is used to synchronize between both.

Parameters

~camera_height (double, default: 1.0)

• Height of the camera above the ground in meters.

~camera_pitch (double, default: 0.0)

• Pitch of the camera in radiants, negative pitch means looking downwards.

~ransac_iters (int, default: 2000)

• Number of RANSAC iterations.

~inlier_threshold (double, default: 0.00001)

• Fundamental matrix inlier threshold.

~motion_threshold (double, default: 100.0)

• Threshold for stable fundamental matrix estimation.

stereo_odometer

Subscribed Topics

<stereo>/left/<image> (sensor_msgs/Image)

• Left rectified input image.

<stereo>/right/<image> (sensor_msgs/Image)

• Right rectified input image.

<stereo>/left/camera_info (sensor_msgs/CameraInfo)

• Camera info for left image.

<stereo>/right/camera_info (sensor_msgs/CameraInfo)

• Camera info for right image.

Published Topics

~point_cloud (sensor_msgs/PointCloud2)

• Point cloud formed by the matched features.

Parameters

~queue_size (int, default: 5)

• Length of the input queues for left and right camera synchronization.

~approximate_sync (bool, default: false)

• Approximate synchronization of incoming messages, set to true if cameras do not have synchronized timestamps.

~ransac_iters (int, default: 200)

• Number of RANSAC iterations.

~inlier_threshold (double, default: 1.5)

• Fundamental matrix inlier threshold.

~reweighting (bool, default: true)

• Lower border weights (more robust to calibration errors).

~ref_frame_change_method (int, default: 0)

• Defines the method of reference frame change for drift compensation. 0 means reference frame is changed for every algorithm iteration. 1 changes the reference frame if last motion is small (ref_frame_motion_threshold param). 2 changes the reference frame if the number of inliers is smaller than ref_frame_inlier_threshold param.

~ref_frame_motion_threshold (double, default: 5.0)

• If the mean movement in pixels of all features lies below this threshold, the reference image inside the odometer will not be changed.

~ref_frame_inlier_threshold (int, default: 150)

• If the number of inliers between current frame and reference frame is smaller than this threshold, the reference image inside the odometer will be changed.

Troubleshoting

If you have a problem, please look if it is stated here or on ROS Answers (FAQ link above) and you can solve it on your own.

I run mono_odometer but I get no messages on the output topics

• Check if incoming image and camera_info messages are synchronized.
• Move the camera. To estimate motion the mono odometer actually needs some motion (else the estimation of the F-matrix is degenerating). The system needs the camera to perform a translation, pure rotation will not work.

• Set the log level of mono_odometer to DEBUG (e.g. using rxconsole) and look if you can find something.

Feedback

Please use the stack's issue tracker at Github to submit bug reports and feature requests regarding the ROS wrapper of libviso2: https://github.com/srv/viso2/issues/new.