Structure From Motion (SFM) module for estimation of estrinsics parameter and computation of depth map.

Structure From Motion (SFM) module for estimation of estrinsics parameter and computation of depth map.

Copyright (C) 2015 RobotCub Consortium

Author: Sean Ryan Fanello, Giulia Pasquale

Date: first release around 24/07/2013

CopyPolicy: Released under the terms of the GNU GPL v2.0.

Description

The module uses a complete Structure From Motion (SFM) pipeline for the computation of the extrinsics parameters between two different views. These parameters are then used to rectify the images and to compute a depth map using either the H. Hirschmuller Algorithm (CVPR 2006), implemented since Opencv 2.2, or LIBELAS (Library for Efficient Large-scale Stereo Matching). The Kinematics of the iCub is used to guess the current camera positions, then visual features are used to refine this model. Before starting, make sure you have calibrated the intrinsics parameters. For the stereo calibration see the module stereoCalib. This module provides six output ports: the first one is the disparity map in grayscale values, the second and the third port are the WorldImage, which are a 3-channels float images, where in each pixel are stored the three Cartesian and the Cylindrical coordinates with respect to robot root reference frame. The fourth port outputs the current keypoints match. Non valid points are handled with the special value (0,0,0). The last two ports output the rectified images used to compute the horizontal disparity map. In addition, a rpc port supports requests for 3D/2D points computation (see below).

Note

If you're going to use this module for your work, please quote it within any resulting publication: Fanello S.R., Pattacini U., Gori I., Tikhanoff V., Randazzo M., Roncone A., Odone F., Metta G., "3D Stereo Estimation and Fully Automated Learning of Eye-Hand Coordination in Humanoid Robots", Proceedings of IEEE-RAS International Conference on Humanoid Robots, Madrid, Spain, November 18-20, 2014.

Libraries

YARP libraries and OpenCV 2.4 (at least).
For better performance, we suggest you to run the module on a machine equipped with GPU functionality along with the SiftGPU library installed. This module now uses LIBELAS by default to compute the horizontal disparity map from the rectified left and right images. The source code of LIBELAS is compiled with the stereoVisionLib (with no particular dependences). The OpenMP accelerated version of the LIBELAS is used under UNIX systems, if OpenMP is available.

Parameters

–name SFM

• The parameter stemName specifies the stem name of ports created by the module.

–from stereoCalibFile

• The parameter stereoCalibFile specifies the stereo calibration file to configure the module with. Default is icubEyes.ini

–context stereoCalibContext

• The parameter stereoCalibContext defines the YARP context to search for the stereo calibration file in. Default is cameraCalibration

–robot robotName

• The parameter robotName specifies the name of the robot.

–leftPort /left:i

• The parameter inputLeft specifies the left image input port.

–rightPort /right:i

• The parameter inputRight specifies the right image input port.

–outLeftRectImgPort /rect_left:o

• Specifies the left rectified image output port.

–outRightRectImgPort /rect_right:o

• Specifies the right rectified image output port.

–outDispPort /disp:o

• The parameter /disparity:o specifies the output port for the disparity image.

–outMatchPort /match:o

• The parameter /match:o specifies the output port for the match image.

–outWorldPort /world

• The parameter /world specifies the output suffix for the world images. The final tags /cartesian:o and /cylindrical:o are appended.

–CommandPort comm

• The parameter comm specifies the command port for rpc protocol.

–skipBLF

• Disable Bilateral filter.

–use_sgbm

• By default LIBELAS is used to compute the disparity. However, if you prefer to continue using the OpenCV's SGBM algorithm, you just need to pass the parameter use_sgbm.

If you use LIBELAS, there is the possibility of setting the following parameters:

–disp_scaling_factor 1.0

• This parameter provides the option of resizing the left and right images before computing the disparity map (and finally resize again the resulting map to the original size). It is a multiplicative factor. For example, if a low-resolution (also less accurate!) disparity map is sufficient, but needed at high rate, you can set this parameter to 1/N so that the images width and height are divided by a factor N, LIBELAS computes the disparity of the downsampled images, and finally the disparity map's size is rescaled by N before returning.

–elas_setting ROBOTICS

• The parameter ROBOTICS or MIDDLEBURY allow to choose between the two settings of parameters defined in elas.h. This module gives the possibility of modifying (eventually tuning to individual needs) those parameters which differ between the two settings, plus a couple of others (elas_subsampling and elas_add_corners). The remaining parameters are supposed to be fixed to the values proposed by the authors of LIBELAS.

Here we list those LIBELAS parameters that can be passed to this module; see elas.h for the complete list of parameters, their definitions and default values.

–elas_subsampling

• Pass the parameter elas_subsampling if you want to set the subsampling parameter to true in elas.h, to speedup the computation (at the expenses of accuracy).

–elas_add_corners

• Pass the parameter elas_add_corners if you want to set add_corners parameter to true in elas.h, to consider also the image corners in the computation of the disparity map.

–elas_ipol_gap_width 40

• This is the only parameter for which we set a default value different from the ones provided in elas.h, where the ipol_gap_width parameter is set to 3 in ROBOTICS and 5000 in MIDDLEBURY. It is the radius of interpolation (in pixel) of the disparity values found in the keypoints. Small values result in sparser disparity maps (with more black holes); high values result in denser maps, with the black regions filled with interpolated values.

Also the following LIBELAS parameters can be modified by the user, however we stick with the values provided by the authors.

–elas_support_threshold 0.85

• This is the support_threshold parameter in elas.h, set to 0.95 in MIDDLEBURY and 0.85 in ROBOTICS.

–elas_gamma 3

• This is the gamma parameter in elas.h, set to 5 in MIDDLEBURY and 3 in ROBOTICS.

–elas_sradius 2

• This is the sradius parameter in elas.h, set to 3 in MIDDLEBURY and 2 in ROBOTICS.

–elas_match_texture true

• This is the match_texture parameter in elas.h, set to false in MIDDLEBURY and true in ROBOTICS.

–elas_filter_median false

• This is the filter_median parameter in elas.h, set to true in MIDDLEBURY and false in ROBOTICS.

–elas_filter_adaptive_mean true

• This is the filter_adaptive_mean parameter in elas.h, set to false in MIDDLEBURY and true in ROBOTICS.

Ports Created

• /SFM/left:i accepts the incoming images from the left eye.
• /SFM/right:i accepts the incoming images from the right eye.
• /SFM/disp:o outputs the disparity map in grayscale values.
• /SFM/world/cartesian:o outputs the world image (3-channel float with X Y Z values).
• /SFM/world/cylindrical:o outputs the world image (3-channel float with R Theta Z values).
• /SFM/match:o outputs the match image.
• /SFM/rect_left:o outputs the rectified left image.
• /SFM/rect_right:o outputs the rectified right image.
• /SFM/rpc for terminal commands communication.
  • [calibrate]: It recomputes the camera positions once.
  • [save]: It saves the current camera positions and uses it when the module starts.
  • [getH]: It returns the calibrated stereo matrix.
  • [setNumDisp NumOfDisparities]: It sets the expected number of disparity (in pixel). Values must be divisible by 32. Good values are 64 for 320x240 images and 128 for 640x480 images.
  • [setMinDisp minDisparity]: It sets the minimum disparity (in pixel).
  • [Point x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the LEFT eye. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).
  • [x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z ur vr computed using the depth map wrt the the ROOT reference system; (ur vr) is the corresponding pixel in the Right image. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).
  • [Left x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the LEFT eye. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).
  • [Right x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the RIGHT eye. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).
  • [Root x y]: Given the pixel coordinate x,y in the Left image the response is the 3D Point: X Y Z computed using the depth map wrt the ROOT reference system. Points with non valid disparity (i.e. occlusions) are handled with the value (0.0,0.0,0.0).
  • [Rect tlx tly w h step]: Given the pixels in the rectangle defined by {(tlx,tly) (tlx+w,tly+h)} (parsed by columns), the response contains the corresponding 3D points in the ROOT frame. The optional parameter step defines the sampling quantum; by default step=1.
  • [Points u_1 v_1 ... u_n v_n]: Given a list of n pixels, the response contains the corresponding 3D points in the ROOT frame.
  • [Flood3D x y dist]: Perform 3D flood-fill on the seed point (x,y), returning the following info: [u_1 v_1 x_1 y_1 z_1 ...]. The optional parameter dist expressed in meters regulates the fill (by default = 0.004).
  • [uL_1 vL_1 uR_1 vR_1 ... uL_n vL_n uR_n vR_n]: Given n quadruples uL_i vL_i uR_i vR_i, where uL_i vL_i are the pixel coordinates in the Left image and uR_i vR_i are the coordinates of the matched pixel in the Right image, the response is a set of 3D points (X1 Y1 Z1 ... Xn Yn Zn) wrt the ROOT reference system.
  • [cart2stereo X Y Z]: Given a world point X Y Z wrt to ROOT reference frame the response is the projection (uL vL uR vR) in the Left and Right images.
  • [doBLF flag]: activate Bilateral filter for flag = true, and skip it for flag = false (default by config).
  • [bilatfilt sigmaColor sigmaSpace]: Set the parameters for the bilateral filer (default sigmaColor = 10.0, sigmaSpace = 10.0 .

Input Data Files

None.

Output Data Files

None.

Tested OS

Linux (Ubuntu 9.04, Debian Squeeze) and Windows 7. Tested against OpenCV versions: 2.4.

Author

Sean Ryan Fanello, Giulia Pasquale