Binocular camera calculates dense depth point cloud
The main content of this tutorial is to introduce how to use binocular cameras to calculate dense 3D point clouds. It mainly involves the use of elas package, which is tested through KITTI data and ZED camera.
In robot navigation, the depth map is an important data for generating dense 3D environment maps. In indoor robots, RGB-D cameras (such as kinect) based on the principle of active infrared distance measurement are generally used to obtain depth images. RGB-D cameras are due to their measurement range and principle. It is mainly suitable for indoor environment. In an outdoor environment, since the binocular camera has no scale problem, the binocular camera is usually used to calculate the depth map.
1. Principle of binocular stereo matching
First of all, we first introduce some procedures of the name and stereo matching of binocular images in binocular stereo matching.
Picture from [1]
Several definitions are involved here:
Baseline : the connection between the left and right optical centers.
Pole : The intersection of the image plane and the baseline is called the pole. The
line between the image point and the pole is called the
polar plane . The plane of is the polar plane corresponding to the space point
Using the binocular camera to calculate the dense depth requires 3 steps: (image correction, parallax calculation)
1.1 Image correction
Step 1 : Image correction, this part can be subdivided into image distortion correction and image stereo matching
Image de-distortion is to use the image distortion parameter [d1 d2 d3 p1 p2] to correct the image distortion. The detection method is that the original straight line at the edge of the image becomes a curve during imaging due to the distortion of the camera. After the distortion correction , Becomes a straight line in the image, as shown in the following figure:
This is also called the camera's internal parameter calibration (focal length fx fy camera center cx cy), usually we can call the package in ROS to achieve.
Step 2 : Stereo distortion correction
Stereo matching refers to rotating the left and right images to the same plane (the same plane is not very accurate here, it should be two parallel planes) so that the optical axes of the left and right pictures are parallel. The detection method is to judge the left and right images. Whether the same pixel is on the same horizontal line.
Stereo correction is achieved by calibrating the external parameters of the camera. This can also be achieved through the camera calibration package under ROS. You only need to enter the size of the checkerboard. The calibration parameters include (baseline fx camera R matrix projection matrix P matrix)
After obtaining the camera's internal and external parameters, we can call the function of opencv to correct the binocular image, as shown below:
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
cv::remap(img_left,imLeft,M1l,M2l,cv::INTER_LINEAR);
cv::remap(img_right,imRight,M1r,M2r,cv::INTER_LINEAR);
There is another way to implement it through the package that comes with ROS. The following is to start two nodes through the launch file to process the left and right images respectively
<launch>
<!-- If you already have rectified camera images remove the image_proc nodes -->
<group ns="stereo/left">
<node name="left_rect" pkg="image_proc" type="image_proc"/>
</group>
<group ns="stereo/right">
<node name="right_rect" pkg="image_proc" type="image_proc"/>
</group>
</launch>
1.2 Parallax calculation
The disparity is defined as the pixel coordinates of the feature point p in the left-eye image minus the pixel coordinates of the right-eye image: disparity=xl-xr , here only the difference of the abscissa is calculated, because the input image is considered to be an image that has been stereo corrected. The left and right eyes are the same pixel on a straight line. The relationship between depth and disparity conversion: depth = (fx * baseline) / disparity , where baseline represents the length of the baseline.
The essence of binocular stereo matching means that if the left and right eye images collected by the binocular camera are used at the same time, a 3D point cloud image is generated.
There are two ROS packages for binocular calculation of deep point clouds: stereo_image_proc and LIBELAS (Library for Efficient Large-scale Stereo Matching). LIBELAS provides a package elas_ros in the ROS environment that can be used directly under ROS.
Compared with stereo_image_proc, the depth calculated by the ELAS library is more dense. It is recommended to use the ELAS library.
2. Run the elas_ros package
ELAS [2] is an effective stereo matching algorithm based on a probability model. The algorithm has two assumptions: (I) certain stereo correspondences can be reliably matched, and (II) the disparity of pixel i is independent of all other pixels. Therefore, ELAS first uses SAD matching on gradient images to calculate a sparse support point set, and then uses Delaunay triangulation to construct a 2D mesh for dense matching. Finally, the maximum a posteriori (MAP) model is used to estimate the disparity. If you are interested in the article, you can read the following big guys to accelerate the ELAS algorithm on FPGA[3], and open source the code [4] .
1. First download the code package on the github address of elas_ros, and then compile
cd ~/catkin_ws/src
git clone https://github.com/jeffdelmerico/cyphy-elas-ros.git
cd ..
catkin_make
2. Refer to the elas.launch file under the launch folder in elas_ros, create a new folder named dataset.launch, and modify the parameter "stereo" in the fourth line of the launch file to "narrow_stereo_textured". The modified dataset.launch file content as follows:
<launch>
<!-- Launches ELAS node, and rectification nodes for input -->
<!-- Arguments: input stereo namespace and output elas namespace -->
<arg name="stereo" default="narrow_stereo_textured"/>
<arg name="elas_ns" default="elas"/>
<!-- If you already have rectified camera images remove the image_proc nodes -->
<group ns="$(arg stereo)/left">
<node name="left_rect" pkg="image_proc" type="image_proc"/>
</group>
<group ns="$(arg stereo)/right">
<node name="right_rect" pkg="image_proc" type="image_proc"/>
</group>
<!-- This node actually does the stereo reconstruction -->
<node name="$(arg elas_ns)" pkg="elas_ros" type="elas_ros" output="screen">
<remap from="stereo" to="$(arg stereo)"/>
<remap from="image" to="image_rect"/>
<param name="approximate_sync" value="true" />
<param name="disp_min" type="int" value="0"/>
<param name="disp_max" type="int" value="255"/>
<param name="support_threshold" type="double" value="0.95"/>
<param name="support_texture" type="int" value="10"/>
<param name="candidate_stepsize" type="int" value="5"/>
<param name="incon_window_size" type="int" value="5"/>
<param name="incon_threshold" type="int" value="5"/>
<param name="incon_min_support" type="int" value="5"/>
<param name="add_corners" type="bool" value="0"/>
<param name="grid_size" type="int" value="20"/>
<param name="beta" type="double" value="0.02"/>
<param name="gamma" type="double" value="3"/>
<param name="sigma" type="double" value="1"/>
<param name="sradius" type="double" value="2"/>
<param name="match_texture" type="int" value="1"/>
<param name="lr_threshold" type="int" value="2"/>
<param name="speckle_sim_threshold" type="double" value="1"/>
<param name="speckle_size" type="int" value="200"/>
<param name="ipol_gap_width" type="int" value="300"/>
<param name="filter_median" type="bool" value="0"/>
<param name="filter_adaptive_mean" type="bool" value="1"/>
<param name="postprocess_only_left" type="bool" value="1"/>
<param name="subsampling" type="bool" value="0"/>
<!-- If your cameras are not synchronised then uncomment the following line -->
<!-- <param name="approximate_sync" value="true" type="bool"/> -->
</node>
</launch>
3. Download the test data set "rotating_detergent_1_6.bag" we used, ( Baidu network disk link : Extraction code: e9ga) Data set source address
4. Create three new terminals, and then start the node
- Start elas_ros
roslaunch elas_ros dataset.launch
- Play rosbag
rosbag play rotating_detergent_1_6.bag
- Start rviz
rosrun rviz rviz
Add the corresponding topic in RVIZ, you can see the disparity map and point cloud map:
elas binocular point cloud mapping
3. KITTI data set operation
3.1 Convert kitti data set to rosbag
In the process of running the binocular deep talk, we first need to convert the KITTI data set to rosbag for use. To convert kitti data set to rosbag, please refer to the blog: KITTI data set test-1 From KITTI data to rosbag
python img2bag_kitti_odo.py /your directory/KITTI/dataset/sequences/00/image_0 kitti_00.bag /your directory/KITTI/dataset/sequences/00/times.txt
In addition, there are two packages that can also achieve this function. The first is the kitti_to_rosbag package of ethz-asl , but kitti_to_rosbag needs to rely on a lot of packages. I did not use this package. Interested friends can study it. The other is kitti2bag .
Here you need to pay attention to adding the baseline information between the two eyes (usually adding bf=baseline*fx to the P matrix). Secondly, the KITTI data I used is the aligned data set, which is on the Odometry webpage of KITTI official website. The image in it is the image after stereo correction, so when we use the kitti data set, we don’t need to include Image stereo correction package
3.2 Run KITTI data set
Create a launch file named kitti.launch and write the following
<launch>
<!-- Launches ELAS node, and rectification nodes for input -->
<!-- Arguments: input stereo namespace and output elas namespace -->
<arg name="stereo" default="camera"/>
<arg name="elas_ns" default="elas"/>
<!-- If you already have rectified camera images remove the image_proc nodes -->
<group ns="$(arg stereo)/left">
<node name="left_rect" pkg="image_proc" type="image_proc"/>
</group>
<group ns="$(arg stereo)/right">
<node name="right_rect" pkg="image_proc" type="image_proc"/>
</group>
<!-- This node actually does the stereo reconstruction -->
<node name="$(arg elas_ns)" pkg="elas_ros" type="elas_ros" output="screen">
<remap from="stereo" to="$(arg stereo)"/>
<remap from="image" to="image_rect"/>
<param name="disp_min" type="int" value="0"/>
<param name="disp_max" type="int" value="255"/>
<param name="support_threshold" type="double" value="0.95"/>
<param name="support_texture" type="int" value="10"/>
<param name="candidate_stepsize" type="int" value="5"/>
<param name="incon_window_size" type="int" value="5"/>
<param name="incon_threshold" type="int" value="5"/>
<param name="incon_min_support" type="int" value="5"/>
<param name="add_corners" type="bool" value="0"/>
<param name="grid_size" type="int" value="20"/>
<param name="beta" type="double" value="0.02"/>
<param name="gamma" type="double" value="3"/>
<param name="sigma" type="double" value="1"/>
<param name="sradius" type="double" value="2"/>
<param name="match_texture" type="int" value="1"/>
<param name="lr_threshold" type="int" value="2"/>
<param name="speckle_sim_threshold" type="double" value="1"/>
<param name="speckle_size" type="int" value="200"/>
<param name="ipol_gap_width" type="int" value="300"/>
<param name="filter_median" type="bool" value="0"/>
<param name="filter_adaptive_mean" type="bool" value="1"/>
<param name="postprocess_only_left" type="bool" value="1"/>
<param name="subsampling" type="bool" value="0"/>
<param name="approximate_sync" value="true" />
<param name="queue_size" type="int" value="5"/>
<!-- If your cameras are not synchronised then uncomment the following line -->
<!-- <param name="approximate_sync" value="true" type="bool"/> -->
</node>
<!--node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find elas_ros)/rviz/KITTI.rviz" / -->
</launch>
Note: In lines 7-14 in the above launch file, I am lazy here to not modify the topic. I still retain this image stereo correction. Actually, you don’t need to use this code if you input the corrected image. Up.
Next, play the rosbag corresponding to kitti and start elas_ros:
roslaunch elas_ros kitti.launch
rosbag play kitti_01.bag
In this way, you can see the binocular point cloud built in real time using the elas_ros package. This package can run at a frequency of about 10HZ, so it is best to use keyframes when used for mapping.
(ELAS operation effect on KITTI data set)
they
Reference
官方教程：https://github.com/MichaelGrupp/evo
[1] https://blog.csdn.net/luoru/article/details/49048287
[2] Geiger A, Roser M, Urtasun R. Efficient large-scale stereo matching[C]//Asian conference on computer vision. Springer, Berlin, Heidelberg, 2010: 25-38.
[3] Rahnama O, Frost D, Miksik O, et al. Real-time dense stereo matching with ELAS on FPGA-accelerated embedded devices[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 2008-2015.
[4] https://github.com/torrvision/ELAS_SoC
[5] Jellal R A, Lange M, Wassermann B, et al. LS-ELAS: Line segment based efficient large scale stereo matching[C]//2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017: 146-152.
If you think the article is helpful to you, please give me a like. O(∩_∩)O
Welcome everyone to exchange and discuss in the comment area ([email protected])
Students who need the code can leave their mailbox in the comment area, and send the code and data test package to the mailbox.