ROS: карта глубин на Raspberry Pi «малой кровью»

    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, предложивший свое решение, и был автором вопроса про публикацию двух стереокартинок. Сам спросил, сам решил.
    • +24
    • 5,2k
    • 9
    Поделиться публикацией

    Похожие публикации

    Комментарии 9
      0
      Отличный обзор!
      Подскажите, с одной камерой на RPI удовлетворительные результаты можно получить? (Что-то вроде MonoSLAM)
        +1
        Насколько я понимаю с одной камерой нужно выхватывать оптический поток при движении робота, я в этой области экспериментов не проводил. Несколько лет назад пробовал стереозрение с одной камерой (https://habr.com/post/388259/), потом проект вырос в StereoPi, а прошлый был сделан полностью опренсорсным (включая чертежи).
          +1
          если использовать нейронки, какие нибудь 5 кадров в секунду… но качество слабое
            0
            Гугл выкатывал набор с пизирой и нейронами для баловства — там вроде даже приемлемая производительность. Соглашусь с Tremere — хорошо тюнить надо под специфическое применение.
          0
          del
            0
            Какая производительность получилась у вас, сколько кадров секунду, нагрузка на ЦПУ и ОЗУ?
              +1
              Я отдельно пиковые тесты не гонял, была задача проверить что всё это вообще будет работать без бубнов. В настройках у автора скрипта стояло разрешение 320х240 и FPS 10, при таких установках никаких лагов замечено не было. Попробую завтра прогнать без GUI с другими установками. Хотя интереснее будет посмотреть скорость в работе с полноценным стереоскопическим драйвером, надеюсь Rohan сможет его выкатить в ближайшее время.
                0
                Если не сложно, померьте задержку обработки (от изменения обстановки до выдачи результата). Я тут делал простую навигацию по езде в направлении шаблона (шахматной доски), так довольно сильно пришлось извращаться с запоминанием реального направления движения по компасу на какое-то время назад, чтобы не промахнуться из-за задержки в обработке (пока нам вычисляют результат — робот успевает проехать/провернуться немного не туда).
                  0
                  Посмотрел лимит скорости. Пока бутылочное горлышко — это питоновый скрипт, который режет картинку на пары и публикует в виде отдельных камер. У него скорость выдачи плавает от 17 до 25 кадров. Карта глубин при этом отображается синхронно с видео.

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

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

            Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.

            Самое читаемое