26.11.2018 11:18
Realizator 9 4600 Источник
image

Если вы используете ROS при создании роботов, то наверняка знаете, что в ней есть поддержка работы со стереокамерами. Можно построить, например, карту глубин видимой части пространства или облако точек. И мне стало интересно, насколько просто будет использовать в ROS стереокамеру StereoPi на базе малины. Раньше я уже убедился, что карта глубин отлично строится силами OpenCV, но вот с ROS никогда дела не имел. И решил попробовать. Я хочу рассказать о моих приключениях в поиске решения.

1. Бывает ли вообще ROS на Raspberry Pi?


Сначала я решил узнать, есть ли возможность собрать ROS для Raspberry Pi. Первое, что мне подсказал гугл – это список инструкций по установке разных версий ROS на Raspberry Pi, а именно вот эту страничку ROS wiki

Отлично, уже есть от чего отталкиваться! Я хорошо помнил, сколько занимала сборка OpenCV на Raspberry (примерно часов восемь), поэтому решил поискать готовые образы карточек MicroSD для экономии времени.

2. Есть ли готовые образы карточек MicroSD с ROS для Raspberry?


Оказалось, что и этот вопрос решен уже несколькими командами разработчиков. Если не брать разовые сборки энтузиастами, то выделялась парочка образов, которые постоянно обновляются с выходом новых версий ОС и ROS.

Первый вариант – это ROS, установленный на родную ОС Raspbian от команды ROSbots, вот страничка с обновляемой ссылкой на образ: ready-to-use-image-raspbian-stretch-ros-opencv

Второй – это образы от Ubiquiti Robotics на убунте.

Ну что же, второй вопрос тоже был достаточно быстро закрыт. Пора заныривать поглубже.

3. Как в ROS устроена работа с камерой Raspberry Pi?


А какие стереокамеры вообще поддерживаются в ROS? Я посмотрел страничку со стереокамерами, для которых заявлено наличие готовых драйвера под ROS, вот эту:

wiki.ros.org/Sensors

Там нашлось два раздела:
2.3 3D Sensors (range finders & RGB-D cameras)
2.5 Cameras
Оказалось, что в первом разделе указаны не только стереокамеры, но и TOF сенсоры, и сканирующие лидары — в общем всё, что сразу может давать информацию в 3D. А во втором уже идут стереокамеры. Попытка посмотреть драйвера к нескольким стереокамерам радости мне не прибавила, так как намекала на серьезное погружение в код.

Хорошо, отступим на шаг назад. Как идет работа с одной камерой Raspberry Pi в ROS?

Тут меня ждало три приятных сюрприза:

  • оказывается, для ROS есть специальная нода raspicam_node как раз для работы с камерой Raspberry Pi
  • сорсы ноды лежат на гитхабе, код активно поддерживается и хорошо документирован: github.com/UbiquityRobotics/raspicam_node
  • автор ноды Rohan Agrawal (@Rohbotics) работает в компании, которая активно поддерживает один из готовых образов для Raspberry Pi

Я посмотрел гитхабовский репозиторий raspicam_node и заглянул в issues. Там я нашел открытую issue с ёмким названием «stereo mode» почти семимесячной давности, без ответов и комментариев. Собственно, в ней дальше и развивались все события.

4. Хардкор или нет?


Чтобы не задавать детских вопросов авторам я решил посмотреть исходники и оценить, чем грозит добавление стереорежима. Меня больше заинтересовала сишная часть вот тут: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src
Ну что, ребята написали драйвер погружаясь на уровень MMAL. Я также вспомнил, что исходники по работе малины в стереорежиме тоже открыты (эволюцию можно проследить вот тут на форуме малины), и задача написания полноценного стереодрайвера решаема, но масштабна. Глянув на описание драйверов других камер я понял, что необходимо публиковать не только левую и правую картинки, но и выдавать параметры обеих камер, применять к каждой свои результаты калибровки и делать много других вещей. Это тянуло на эксперименты длиной в месяц-два. Поэтому я решил распараллелить подход, а именно: написать автору вопрос о поддержке стерео, а самому поискать более простое, но работающее решение.

5. Диалоги с автором


В ветке про стереорежим на гитхабе я задал вопрос автору, упомянув, что стерео поддерживается малиной аж с 2014 года, и предложил в случае надобности выслать ему отладочную плату для экспериментов. Напомню, что я все еще сомневался в том, что в данном дистрибутиве стерео будет работать как в родном Raspbian.

Rohan ответил на удивление быстро, сообщив, что их дистриб использует малиновое ядро и все должно работать. И попросил проверить это на одной из их сборок.

Малиновое ядро! Ура! Теоретически стереокартинка должна захватываться без танцев с бубном!

Я скачал и развернул их последний образ по ссылке от Rohan и запустил простенький скрипт на питоне по захвату стереопары. Оно работало!

image

После этого Rohan написал, что он глянет код драйвера на предмет стереорежима, и написал парочку вопросов. Например, у нас стереорежим выдает одну склеенную картинку, а нам бы надо ее резать на две – левую и правую. И второй вопрос о параметрах калибровки каждой камеры – как с этим быть.

Я сказал, что в качестве первого шага можно брать картинки с камер независимо. Да, они не будут синхронизированы по времени захвата и настройкам (типа яркости-контрастности-баланса белого), но в качестве первого шага это может вполне сойти.

Rohan оперативно выкатил патч, позволяющий непосредственно из ROS указывать, с какой из камер брать картинки. Я его проверил – выбор камеры работает, уже отличный результат.

6. Неожиданная помощь


И тут в ветке появляется комментарий от юзера Wezzoid. Он рассказал, что делал проект на базе стереокамеры на Pi Compute 3 с использованием малиновой девборды. Его четырехногий шагающий робот трекал положение объекта в пространстве, менял положение камер и держал заданное расстояние до него (проект выложен на hackaday.io тут ).

image

И он поделился кодом, в котором он захватывал картинку, резал ее питоновыми средствами пополам и публиколвал как ноды левой и правой камер.
Питон в этих вопросах не очень быстрый товарищ, поэтому он использовал невысокое разрешение 320х240 и хороший лайфхак. Если мы захватываем стереокартинку side-by-sibe (одна камера слева на стереокартинке, вторая справа), то питон должен порезать каждую из 240 строчек пополам. А вот если сделать картинку top-bottom (левая камера – верхняя половина кадра, правая – нижняя), то питон режет массив пополам за одну операцию. Что и было успешно сделано юзером Wezzoid.
Плюс он выложил свой питоновый код на Pastebin, который проделывал эту операцию. Вот он:

Код Wezzoid по публикации нод двух камер из стереопары
#!/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)


7. Запускаем публикацию нод левой и правой камер


При первом запуске код ругнулся, что нету доступа к YML файлам с параметрами камер. Я использовал малиновые камеры V2 и помнил, что на гитхабе в комплекте к raspicam_node шли готовые файлики с результатами калибровки для разных моделей камер: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info
Я взял один из них, сделал две копии и сохранил с именами left.yml и right.yml, прописав в них разрешение камер из скрипта автора. Вот что получилось для левой камеры:

left.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. В остальном файл идентичен.

Так как я не планировал делать сложный проект, я не стал повторять длинные пути автора с вложенными подпапками и просто положил файлики в корень домашней папки рядом с питоновым скриптом. Код успешно стартовал, выводя в консоли статусные сообщения.

image

Оставалось только посмотреть, что же в итоге публикуется нашими левой и правой камерами. Для этого я запустил rqt_image_view. В выпадающем меню появились пункты /left/image_raw и /right/image_raw, при выборе которых я видел изображения с левой и правой камер.

image

Ну что же, эта штука заработала! Теперь самое интересное.

8. Смотрим карту глубин.


Для просмотра карты глубин я не стал придумывать свой подход и пошел по классическому мануалу из ROS по настройке стереопараметров.
Оттуда я выяснил, что хорошо бы публиковать обе ноды в определенном 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

В итоге видим картинку, приведенную в самом начале статьи. Вот фрагмент чуть крупнее:

image

Все файлики я выложил на гитхабе: github.com/realizator/StereoPi-ROS-depth-map-test

9. Ближайшие планы


После моей публикации результата в дискуссии на гитхабе Rohan написал «Круто! Походу нужно мне забрать свою StereoPi». Мы списались с ним по почте, я отправил ему плату. Надеюсь, что с работающим железом в руках ему будет проще допилить и отладить полноценный стереодрайвер для ROS и Raspberry.

10. Итоги


Карту глубин из стереокартинки на малине в ROS можно получить, причем несколькими путями. Выбранный для быстрой проверки путь является не самым оптимальным в плане производительности, но может использоваться в прикладных целях. Прелесть в его простоте и возможности сразу начать эксперименты.

Ну и из забавного: уже после получения результатов я заметил что Wezzoid, предложивший свое решение, и был автором вопроса про публикацию двух стереокартинок. Сам спросил, сам решил.

Комментарии (9)


  1. a_freeman
    26.11.2018 15:15
    #19416508

    Отличный обзор!
    Подскажите, с одной камерой на RPI удовлетворительные результаты можно получить? (Что-то вроде MonoSLAM)


    1. Realizator Автор
      26.11.2018 15:40
      #19416678

      Насколько я понимаю с одной камерой нужно выхватывать оптический поток при движении робота, я в этой области экспериментов не проводил. Я несколько лет назад пробовал стереозрение с одной камерой (https://habr.com/post/388259/), потом проект вырос в StereoPi, а прошлый я сделал полностью опренсорсным (включая чертежи).


    1. Tremere
      26.11.2018 16:15
      #19416860
      +1

      если использовать нейронки, какие нибудь 5 кадров в секунду… но качество слабое


      1. Realizator Автор
        26.11.2018 16:18
        #19416882

        Гугл выкатывал набор с пизирой и нейронами для баловства — там вроде даже приемлемая производительность. Соглашусь с Tremere — хорошо тюнить надо под специфическое применение.


  1. Realizator Автор
    26.11.2018 15:40
    #19416672

    del


  1. Tremere
    26.11.2018 16:21
    #19416898

    Какая производительность получилась у вас, сколько кадров секунду, нагрузка на ЦПУ и ОЗУ?


    1. Realizator Автор
      26.11.2018 17:42
      #19417228

      Я отдельно пиковые тесты не гонял, была задача проверить что всё это вообще будет работать без бубнов. В настройках у автора скрипта стояло разрешение 320х240 и FPS 10, при таких установках никаких лагов замечено не было. Попробую завтра прогнать без GUI с другими установками. Хотя интереснее будет посмотреть скорость в работе с полноценным стереоскопическим драйвером, надеюсь Rohan сможет его выкатить в ближайшее время.


      1. vasimv
        26.11.2018 23:27
        #19418524

        Если не сложно, померьте задержку обработки (от изменения обстановки до выдачи результата). Я тут делал простую навигацию по езде в направлении шаблона (шахматной доски), так довольно сильно пришлось извращаться с запоминанием реального направления движения по компасу на какое-то время назад, чтобы не промахнуться из-за задержки в обработке (пока нам вычисляют результат — робот успевает проехать/провернуться немного не туда).


        1. Realizator Автор
          27.11.2018 15:42
          #19421498

          Посмотрел лимит скорости. Пока бутылочное горлышко — это питоновый скрипт, который режет картинку на пары и публикует в виде отдельных камер. У него скорость выдачи плавает от 17 до 25 кадров. Карта глубин при этом отображается синхронно с видео.

          Касательно задержки в обработке — сейчас как таковой обработки (типа логики поведения и реакций на ситуацию) нет, просто карта глубин строится. На глаз это около 0,2 секунд (между смещением объекта перед камерой и отрисовкой измененной карты глубины).

          Вообще на новых малиновых сенсорах (V2) теоретически можно до 1000 fps выжать, но это скорее игрушки на пике производительности (вот обсуждение на малиновом форуме). Фактически скорость обработки будет упираться, конечно, в проц.