ELAS_ROS算法 在 KITTI数据集 实践

下载KITTI数据集的Odometry的00数据集
下载ELAS_ROS代码编译后增加kitti.launch如下：

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

在ros新建工程xiaoyao后在scripts增加文档doublecam_talk.py如下：

#!/usr/bin/env python
# license removed for brevity
import rospy
from sensor_msgs.msg import Image,CameraInfo
from std_msgs.msg import Header
import cv2
from cv_bridge import CvBridge
import time
import os
w =1241
h =376

#xiaoyao
def build_camera_info(sec_test,frame_str):
        msg_header = Header(stamp=rospy.Time.from_sec(sec_test))#(stamp=rospy.Time.now())
        #msg_header = Header( stamp=rospy.Time.now() )
        msg_header.frame_id = 'narrow_stereo_optical_frame'#frame_str#"camera"
        camera_info = CameraInfo()
        # store info without header
        camera_info.header = msg_header
        camera_info.width = w
        camera_info.height = h
        camera_info.distortion_model = 'plumb_bob'
        cx = 607.1928
        cy = 185.2157
        fx = 718.856
        fy = 718.856
        camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
        camera_info.D = [0, 0, 0, 0, 0]
        camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
        camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
        camera_info.binning_x =1 
        camera_info.binning_y =1 
        #camera_info.roi.do_rectify=True
        return camera_info 

def talker():
     '''
     /camera/left/image_raw
     /camera/right/image_raw
     /camera/left/camera_info
     /camera/right/camera_info
     '''
     pub1 = rospy.Publisher('/narrow_stereo_textured/left/image_raw', Image, queue_size=5)
     pub2 = rospy.Publisher('/narrow_stereo_textured/right/image_raw', Image, queue_size=5)
     pubinfo1 = rospy.Publisher('/narrow_stereo_textured/left/camera_info', CameraInfo, queue_size=5)
     pubinfo2 = rospy.Publisher('/narrow_stereo_textured/right/camera_info', CameraInfo, queue_size=5)

     rospy.init_node('talker', anonymous=True) 
     rate = rospy.Rate(2)
     datadir = '/home/cc/orbslam2/data/00/'
     lpath=datadir+'image_0/'
     rpath=datadir+'image_1/'
     i=0
     f = open(datadir+"times.txt")
     imglist = os.listdir(lpath)
     imglist.sort( key=None, reverse=False)
     #sorted(imglist)
     #print(imglist)
     for imgfile in imglist:
         if rospy.is_shutdown():
              break
	 imgname = imgfile#.split('/')[-1]
         
         limg = cv2.imread(lpath+imgname)
         rimg = cv2.imread(rpath+imgname)
         #print(lpath+imgname,rpath+imgname)
         lineNum = float( f.readline() )

         left_camera_info =  build_camera_info(lineNum,'')        
         right_camera_info = build_camera_info(lineNum,'')
         left_img_msg   = CvBridge().cv2_to_imgmsg(limg, "bgr8")
         right_img_msg  = CvBridge().cv2_to_imgmsg(rimg, "bgr8")

         left_img_msg.header = left_camera_info.header
         right_img_msg.header = right_camera_info.header

         pubinfo1.publish(left_camera_info) 
         pubinfo2.publish(right_camera_info)
         pub1.publish(left_img_msg)
         pub2.publish(right_img_msg)

         rate.sleep()

if __name__ == '__main__':
     try:
         talker()
     except rospy.ROSInterruptException:
         pass

roscore
roslaunch elas_ros kitti.launch
rosrun xiaoyao doublecam_talk.py

rviz显示
下方Add按钮点击后，
查看Image主题 elas_ros/depth/Image 、elas_ros/image_disparity/Image 、 elas/point_cloud(虚拟机好像显示不了)

之前碰壁：
1.camera_info的格式查看可参考：http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html
rostopic echo /camera/right/camera_info
2.
[image_transport] Topics ‘xxx’ and ‘xxx’ do not appear to be synchronized
图片也有header需要跟camera_info的header同步
3.Rectified topic ‘xxx’ requested but camera publishing ‘xxx’ is uncalibrated
kitti需要内参的书写(在header里的fx)