- PVSM.RU - https://www.pvsm.ru -
Если вы используете ROS при создании роботов, то наверняка знаете, что в ней есть поддержка работы со стереокамерами. Можно построить, например, карту глубин видимой части пространства или облако точек. И мне стало интересно, насколько просто будет использовать в ROS стереокамеру StereoPi на базе малины. Раньше я уже убедился, что карта глубин отлично строится силами OpenCV, но вот с ROS никогда дела не имел. И решил попробовать. Я хочу рассказать о моих приключениях в поиске решения.
Сначала я решил узнать, есть ли возможность собрать ROS для Raspberry Pi. Первое, что мне подсказал гугл – это список инструкций по установке разных версий ROS на Raspberry Pi, а именно вот эту страничку ROS wiki [1]
Отлично, уже есть от чего отталкиваться! Я хорошо помнил, сколько занимала сборка OpenCV на Raspberry (примерно часов восемь), поэтому решил поискать готовые образы карточек MicroSD для экономии времени.
Оказалось, что и этот вопрос решен уже несколькими командами разработчиков. Если не брать разовые сборки энтузиастами, то выделялась парочка образов, которые постоянно обновляются с выходом новых версий ОС и ROS.
Первый вариант – это ROS, установленный на родную ОС Raspbian от команды ROSbots, вот страничка с обновляемой ссылкой на образ: ready-to-use-image-raspbian-stretch-ros-opencv [2]
Второй – это образы от Ubiquiti Robotics на убунте [3].
Ну что же, второй вопрос тоже был достаточно быстро закрыт. Пора заныривать поглубже.
А какие стереокамеры вообще поддерживаются в ROS? Я посмотрел страничку со стереокамерами, для которых заявлено наличие готовых драйвера под ROS, вот эту:
Там нашлось два раздела:
2.3 3D Sensors (range finders & RGB-D cameras)
2.5 Cameras
Оказалось, что в первом разделе указаны не только стереокамеры, но и TOF сенсоры, и сканирующие лидары — в общем всё, что сразу может давать информацию в 3D. А во втором уже идут стереокамеры. Попытка посмотреть драйвера к нескольким стереокамерам радости мне не прибавила, так как намекала на серьезное погружение в код.
Хорошо, отступим на шаг назад. Как идет работа с одной камерой Raspberry Pi в ROS?
Тут меня ждало три приятных сюрприза:
Я посмотрел гитхабовский репозиторий raspicam_node и заглянул в issues. Там я нашел открытую issue с ёмким названием «stereo mode» почти семимесячной давности, без ответов и комментариев. Собственно, в ней дальше и развивались все события.
Чтобы не задавать детских вопросов авторам я решил посмотреть исходники и оценить, чем грозит добавление стереорежима. Меня больше заинтересовала сишная часть вот тут: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src [7]
Ну что, ребята написали драйвер погружаясь на уровень MMAL. Я также вспомнил, что исходники по работе малины в стереорежиме тоже открыты (эволюцию можно проследить вот тут на форуме малины [8]), и задача написания полноценного стереодрайвера решаема, но масштабна. Глянув на описание драйверов других камер я понял, что необходимо публиковать не только левую и правую картинки, но и выдавать параметры обеих камер, применять к каждой свои результаты калибровки и делать много других вещей. Это тянуло на эксперименты длиной в месяц-два. Поэтому я решил распараллелить подход, а именно: написать автору вопрос о поддержке стерео, а самому поискать более простое, но работающее решение.
В ветке про стереорежим на гитхабе я задал вопрос автору, упомянув, что стерео поддерживается малиной аж с 2014 года, и предложил в случае надобности выслать ему отладочную плату для экспериментов. Напомню, что я все еще сомневался в том, что в данном дистрибутиве стерео будет работать как в родном Raspbian.
Rohan ответил на удивление быстро, сообщив, что их дистриб использует малиновое ядро и все должно работать. И попросил проверить это на одной из их сборок.
Малиновое ядро! Ура! Теоретически стереокартинка должна захватываться без танцев с бубном!
Я скачал и развернул их последний образ по ссылке от Rohan [9] и запустил простенький скрипт на питоне по захвату стереопары. Оно работало!
После этого Rohan написал, что он глянет код драйвера на предмет стереорежима, и написал парочку вопросов. Например, у нас стереорежим выдает одну склеенную картинку, а нам бы надо ее резать на две – левую и правую. И второй вопрос о параметрах калибровки каждой камеры – как с этим быть.
Я сказал, что в качестве первого шага можно брать картинки с камер независимо. Да, они не будут синхронизированы по времени захвата и настройкам (типа яркости-контрастности-баланса белого), но в качестве первого шага это может вполне сойти.
Rohan оперативно выкатил патч [10], позволяющий непосредственно из ROS указывать, с какой из камер брать картинки. Я его проверил – выбор камеры работает, уже отличный результат.
И тут в ветке появляется комментарий от юзера Wezzoid. Он рассказал, что делал проект на базе стереокамеры на Pi Compute 3 с использованием малиновой девборды. Его четырехногий шагающий робот трекал положение объекта в пространстве, менял положение камер и держал заданное расстояние до него (проект выложен на hackaday.io тут [11] ).
И он поделился кодом, в котором он захватывал картинку, резал ее питоновыми средствами пополам и публиколвал как ноды левой и правой камер.
Питон в этих вопросах не очень быстрый товарищ, поэтому он использовал невысокое разрешение 320х240 и хороший лайфхак. Если мы захватываем стереокартинку side-by-sibe (одна камера слева на стереокартинке, вторая справа), то питон должен порезать каждую из 240 строчек пополам. А вот если сделать картинку top-bottom (левая камера – верхняя половина кадра, правая – нижняя), то питон режет массив пополам за одну операцию. Что и было успешно сделано юзером Wezzoid.
Плюс он выложил свой питоновый код на Pastebin, который проделывал эту операцию. Вот он:
#!/usr/bin/env python
# picamera stereo ROS node using dual CSI Pi CS3 board
# Wes Freeman 2018
# modified from code by Adrian Rosebrock, pyimagesearch.com
# and jensenb, https://gist.github.com/jensenb/7303362
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import rospy
from sensor_msgs.msg import CameraInfo, Image
import yaml
import io
import signal # for ctrl-C handling
import sys
def parse_calibration_yaml(calib_file):
with file(calib_file, 'r') as f:
params = yaml.load(f)
cam_info = CameraInfo()
cam_info.height = params['image_height']
cam_info.width = params['image_width']
cam_info.distortion_model = params['distortion_model']
cam_info.K = params['camera_matrix']['data']
cam_info.D = params['distortion_coefficients']['data']
cam_info.R = params['rectification_matrix']['data']
cam_info.P = params['projection_matrix']['data']
return cam_info
# cam resolution
res_x = 320 #320 # per camera
res_y = 240 #240
target_FPS = 15
# initialize the camera
print "Init camera..."
camera = PiCamera(stereo_mode = 'top-bottom',stereo_decimate=False)
camera.resolution = (res_x, res_y*2) # top-bottom stereo
camera.framerate = target_FPS
# using several camera options can cause instability, hangs after a while
camera.exposure_mode = 'antishake'
#camera.video_stabilization = True # fussy about res?
stream = io.BytesIO()
# ----------------------------------------------------------
#setup the publishers
print "init publishers"
# queue_size should be roughly equal to FPS or that causes lag?
left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)
right_img_pub = rospy.Publisher('right/image_raw', Image, queue_size=1)
left_cam_pub = rospy.Publisher('left/camera_info', CameraInfo, queue_size=1)
right_cam_pub = rospy.Publisher('right/camera_info', CameraInfo, queue_size=1)
rospy.init_node('stereo_pub')
# init messages
left_img_msg = Image()
left_img_msg.height = res_y
left_img_msg.width = res_x
left_img_msg.step = res_x*3 # bytes per row: pixels * channels * bytes per channel (1 normally)
left_img_msg.encoding = 'rgb8'
left_img_msg.header.frame_id = 'stereo_camera' # TF frame
right_img_msg = Image()
right_img_msg.height = res_y
right_img_msg.width = res_x
right_img_msg.step = res_x*3
right_img_msg.encoding = 'rgb8'
right_img_msg.header.frame_id = 'stereo_camera'
imageBytes = res_x*res_y*3
# parse the left and right camera calibration yaml files
left_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/left.yaml')
right_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/right.yaml')
# ---------------------------------------------------------------
# this is supposed to shut down gracefully on CTRL-C but doesn't quite work:
def signal_handler(signal, frame):
print 'CTRL-C caught'
print 'closing camera'
camera.close()
time.sleep(1)
print 'camera closed'
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
#-----------------------------------------------------------
print "Setup done, entering main loop"
framecount=0
frametimer=time.time()
toggle = True
# capture frames from the camera
for frame in camera.capture_continuous(stream, format="rgb", use_video_port=True):
framecount +=1
stamp = rospy.Time.now()
left_img_msg.header.stamp = stamp
right_img_msg.header.stamp = stamp
left_cam_info.header.stamp = stamp
right_cam_info.header.stamp = stamp
left_cam_pub.publish(left_cam_info)
right_cam_pub.publish(right_cam_info)
frameBytes = stream.getvalue()
left_img_msg.data = frameBytes[:imageBytes]
right_img_msg.data = frameBytes[imageBytes:]
#publish the image pair
left_img_pub.publish(left_img_msg)
right_img_pub.publish(right_img_msg)
# console info
if time.time() > frametimer +1.0:
if toggle:
indicator = ' o' # just so it's obviously alive if values aren't changing
else:
indicator = ' -'
toggle = not toggle
print 'approx publish rate:', framecount, 'target FPS:', target_FPS, indicator
frametimer=time.time()
framecount=0
# clear the stream ready for next frame
stream.truncate(0)
stream.seek(0)
При первом запуске код ругнулся, что нету доступа к YML файлам с параметрами камер. Я использовал малиновые камеры V2 и помнил, что на гитхабе в комплекте к raspicam_node шли готовые файлики с результатами калибровки для разных моделей камер: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info [12]
Я взял один из них, сделал две копии и сохранил с именами left.yml и right.yml, прописав в них разрешение камер из скрипта автора. Вот что получилось для левой камеры:
image_width: 320
image_height: 240
camera_name: left
camera_matrix:
rows: 3
cols: 3
data: [1276.704618338571, 0, 634.8876509199106, 0, 1274.342831275509, 379.8318028940378, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.1465167016954302, -0.2847343180128725, 0.00134017721235817, -0.004309553450829512, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [1300.127197265625, 0, 630.215390285608, 0, 0, 1300.670166015625, 380.1702884455881, 0, 0, 0, 1, 0]
Для правой имя камеры заменено на right, а сам файлик назван right.yml. В остальном файл идентичен.
Так как я не планировал делать сложный проект, я не стал повторять длинные пути автора с вложенными подпапками и просто положил файлики в корень домашней папки рядом с питоновым скриптом. Код успешно стартовал, выводя в консоли статусные сообщения.
Оставалось только посмотреть, что же в итоге публикуется нашими левой и правой камерами. Для этого я запустил rqt_image_view. В выпадающем меню появились пункты /left/image_raw и /right/image_raw, при выборе которых я видел изображения с левой и правой камер.
Ну что же, эта штука заработала! Теперь самое интересное.
Для просмотра карты глубин я не стал придумывать свой подход и пошел по классическому мануалу из ROS по настройке стереопараметров [13].
Оттуда я выяснил, что хорошо бы публиковать обе ноды в определенном namespace, а не в корне как это делал Wezzoid. В итоге старые строчки публикации вида
left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)
стали выглядеть так:
left_img_pub = rospy.Publisher('stereo/right/image_raw', Image, queue_size=1)
После этого запускаем ноду обработки стереорежима stereo_image_proc:
ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_ige_proc
Ну и нам же хочется посмотреть на результат, поэтому стартуем смотрелку:
rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color
И для настройки параметров карты глубин запускаем утилиту настройки:
rosrun rqt_reconfigure rqt_reconfigure
В итоге видим картинку, приведенную в самом начале статьи. Вот фрагмент чуть крупнее:
Все файлики я выложил на гитхабе: github.com/realizator/StereoPi-ROS-depth-map-test [14]
После моей публикации результата в дискуссии на гитхабе Rohan написал «Круто! Походу нужно мне забрать свою StereoPi». Мы списались с ним по почте, я отправил ему плату. Надеюсь, что с работающим железом в руках ему будет проще допилить и отладить полноценный стереодрайвер для ROS и Raspberry.
Карту глубин из стереокартинки на малине в ROS можно получить, причем несколькими путями. Выбранный для быстрой проверки путь является не самым оптимальным в плане производительности, но может использоваться в прикладных целях. Прелесть в его простоте и возможности сразу начать эксперименты.
Ну и из забавного: уже после получения результатов я заметил что Wezzoid, предложивший свое решение, и был автором вопроса про публикацию двух стереокартинок. Сам спросил, сам решил.
Автор: Realizator
Источник [15]
Сайт-источник PVSM.RU: https://www.pvsm.ru
Путь до страницы источника: https://www.pvsm.ru/raspberry-pi/300300
Ссылки в тексте:
[1] ROS wiki: http://wiki.ros.org/ROSberryPi/Setting%20up%20ROS%20on%20RaspberryPi
[2] ready-to-use-image-raspbian-stretch-ros-opencv: https://medium.com/@rosbots/ready-to-use-image-raspbian-stretch-ros-opencv-324d6f8dcd96
[3] образы от Ubiquiti Robotics на убунте: https://downloads.ubiquityrobotics.com
[4] wiki.ros.org/Sensors: http://wiki.ros.org/Sensors
[5] github.com/UbiquityRobotics/raspicam_node: https://github.com/UbiquityRobotics/raspicam_node
[6] @Rohbotics: https://github.com/rohbotics
[7] github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src: https://github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src
[8] вот тут на форуме малины: https://www.raspberrypi.org/forums/viewtopic.php?f=43&t=85012&sid=3ccbc2eceb9cfc2d57ff1fa5be46fb02
[9] по ссылке от Rohan: https://downloads.ubiquityrobotics.com/pi.html
[10] выкатил патч: https://github.com/UbiquityRobotics/raspicam_node/pull/55
[11] тут: https://hackaday.io/project/159212-augmented-mekamon-robot
[12] github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info: https://github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info
[13] мануалу из ROS по настройке стереопараметров: http://wiki.ros.org/stereo_image_proc/Tutorials/ChoosingGoodStereoParameters
[14] github.com/realizator/StereoPi-ROS-depth-map-test: https://github.com/realizator/StereoPi-ROS-depth-map-test
[15] Источник: https://habr.com/post/431092/?utm_campaign=431092
Нажмите здесь для печати.