Depth image from rectified stereo images or disparity image
I need a depth image for my project(MoveIt robotic arm with CV). Stereoimageproc package provides rectified stereo images and disparity image. I can view them in RVIZ and rqtimageview.
What I have tried so far
1)As referenced here https://answers.ros.org/question/165836/depth-image-from-a-disparity-image/, I've tried using disparityimageproc package. My launch file looks like that
<launch>
<node pkg="nodelet" type="nodelet" name="stereo_proc" args="manager"/>
<node name="disparity_image_proc" pkg="nodelet" type="nodelet" args="load disparity_image_proc/depth_image stereo_proc" output="screen">
<remap from="/elp/right/camera_info" to="/right/camera_info"/>
<remap from="/elp/left/camera_info" to="/left/camera_info"/>
<remap from="/elp/left/image_rect_color" to="/left/image_rect_color"/>
<remap from="/elp/disparity" to="/disparity"/>
<remap from="/depth_image" to="/depth_image"/>
<param name="approximate_sync" value="true" type="bool"/>
<param name="queue_size" value="20" type="int"/>
</node>
</launch>
I can launch the nodelet, it doesn't produce any errors, but the /depth_image topic is empty.
2)I have tried writing my own Python node that would subscribe to the pair of rectified images and output a depth image. Here's the code I've came up so far
#!/usr/bin/env python
from __future__ import print_function
import roslib
roslib.load_manifest('depth')
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
import message_filters
from cv_bridge import CvBridge, CvBridgeError
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("depth",Image,queue_size=5)
self.bridge = CvBridge()
print("Create subscribers for each topic")
self.left = message_filters.Subscriber("/elp/left/image_rect_color", Image)
self.right = message_filters.Subscriber("/elp/right/image_rect_color", Image)
print("Create sync filter. Use exact or approximate as appropriate.")
self.ts = message_filters.ApproximateTimeSynchronizer([self.left, self.right], queue_size=5, slop=0.1)
print("Registering callback")
self.ts.registerCallback(self.callback)
def callback(self,left,right):
try:
cv_image_left = self.bridge.imgmsg_to_cv2(left, "rgb8")
cv_image_right = self.bridge.imgmsg_to_cv2(right, "rgb8")
cv_image_left_new = cv2.cvtColor(cv_image_left, cv2.COLOR_BGR2GRAY)
cv_image_right_new = cv2.cvtColor(cv_image_right, cv2.COLOR_BGR2GRAY)
except CvBridgeError as e:
print(e)
print("creating stereo image")
stereo = cv2.StereoBM_create(numDisparities=16, blockSize=15)
depth = stereo.compute(cv_image_left_new, cv_image_right_new)
print("showing depth image")
cv2.imshow("Image window", depth)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(depth, "16SC1"))
except CvBridgeError as e:
print(e)
def main(args):
rospy.init_node('image_converter', anonymous=True)
ic = image_converter()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
It runs without errors, but still nothing on the /depth topic. The image_view window hangs after a while. For this one I am not sure if it is something fundamentally wrong with this approach or I just used wrong image formats.
Any help and feedback would be very much appreciated. Another solution for this problem(obtaining depth image from stereo image OR disparity image)would also be very welcome.
Update 1: Here's my rqtgraph with camera driver(elp camera driver), stereoimageproc and disaprityimage_proc running.
Asked by Wintermute on 2018-10-11 11:26:48 UTC
Comments
I recommend trying to solve the first simpler solution, there is no reason why this shouldn't be working. Can you show us the output of
rqt_graph
while the nodes are running to check all the topics are connected properly.Asked by PeteBlackerThe3rd on 2018-10-12 04:37:12 UTC
Added rqt_graph.
Asked by Wintermute on 2018-10-12 08:45:42 UTC
have you found the solution to your Problem ? im having the same issue ?
Asked by aristow on 2022-11-16 08:36:59 UTC