Нынче все технологии «дроностроительства» активно дешевеют. Кроме одной: получение карты окружающего пространства. Тут есть две крайности: либо дорогие лидары (тысячи долларов) и оптические решения для построения карты глубин (много сотен долларов), либо совсем копеечные решения типа ультразвуковых дальномеров.
Поэтому возникла идея на базе недорогой Raspberry Pi с одной камерой сделать решение, которое окажется в пустующей нише и позволит получать карту глубин «за недорого». Причем сделать это на простом языке программирования типа Python, чтобы это было доступно новичкам для экспериментов. Собственно, о своих результатах я и хотел рассказать. Получившиеся скрипты с примерами фоток можно запускать и на десктопе.
Карта глубин с одной камеры.
Сначала пару слов об оптической части. Для построения карты глубин всегда используются две картинки – с левой и правой камер. А у нас в наличии малина с одной камерой. Поэтому был разработан оптический разделитель, который в результате отдает на камеру уже стереопару.
Простым языком – если глянуть на фото, то из чёрной коробочки на вас смотрят два глазка камеры. А на самом деле камера одна. Просто немного оптической магии.
На фото представлена уже двенадцатая итерация устройства. Потребовалось немало времени, чтобы получить надежную стабильную конструкцию, которая при этом еще и обходилась бы недорого. Самой непростой частью являются внутренние зеркала, которые делались под заказ вакуумным напылением алюминия. Если использовать типовые зеркала, у которых отражающий слой находится под стеклом, а не над ним, то в месте их стыка образуется зазор, который радикально портит всю картину.
На чем будем работать
За базу был взят малиновый образ Raspbian Wheezy, поставлен Python 2.7 и OpenCV 2.4, ну и требуемые пакеты по мелочи — matplotlib, numpy и прочие. Все сорсы и ссылка на готовый образ карточки выложены в конце статьи. Описание скриптов в виде уроков можно посмотреть на сайте проекта
Готовим картинку к построению карты глубин
Так как решение у нас не из металла и без сверхточной оптики, то в результате сборки возможны небольшие отклонения от идеальной геометрии. Плюс еще камера у нас к устройству крепится винтиками, поэтому её положение может быть не идеальным. Вопрос с расположением камеры решается вручную, а компенсация «кривизны» сборки конструкции у нас будет делаться программно.
Скрипт первый – выравнивание камеры
Стык зеркал на картинке в идеале должен быть вертикальным и находиться по центру. «На глазок» это сделать тяжеловато, поэтому был сделан первый скрипт. Он просто захватывает картинку с камеры в режиме live preview, выводит её на экран, а по центру в overlay рисует белую полоску, по которой и идет выравнивание. После того как камера правильно сориентирована – затягиваем винтики посильнее, и сборка закончена.
Что интересного в коде первого скрипта
- Код скрипта достаточно простой – ссылка
- Из интересного и полезного можно отметить работу с оверлеем на малине – эта фича позволяет оперативно выводить на экран информацию, не прибегая к рисованию окошек через cv.imshow() В оверлей можно вывести как картинку, так и массив данных. Для рисования полоски просто я воспользовался массивом, заполнив одну колонку белыми точками. В последнем скрипте, где критична скорость, в оверлей будет выводиться уже карта глубин.
- Технический момент – в нашей схеме используется однократное отражение картинки от зеркала, т.е. она получается перевернутой по горизонтали. Поэтому мы «разворачиваем» её обратно, указав camera.hflip = True
Скрипт второй – получаем «чистую» стереопару
Наша левая и правая картинки стыкуются в центре изображения. Стык на фото имеет ненулевую ширину – от нее можно избавиться только удаляя зеркала от устройства, что увеличивает размер конструкции. Камера малины имеет настроенный на бесконечность фокус, и близко расположенные объекты (в нашем случае это стык) просто «размываются». Поэтому нужно просто указать скрипту, что по нашему мнению является «плохой» зоной, чтобы стереопара резалась чистые на картинки. Был сделан второй скрипт, который выводит картинку и позволяет клавишами указать ту зону, которая будет вырезана.
Вот как выглядит процесс:
Что интересного в коде второго скрипта
- Вот исходники второго скрипта
- Ловим клавиши, нажатые пользователем, и рисуем прямоугольник через cv2.rectangle(). Сюрпризом оказалось то, что коды клавиш могут отличаться в зависимости от режима клавиатуры, ну и на маке есть свои особенности. В итоге, например, для обработки клавиши Enter ловятся три разных варианта кода.
- После настройки может оказаться, что левая и правая части фото имеют разную ширину. Поэтому скрипт выбирает наименьшую из них и дальше работает с ней, чтобы левое и правое изображения имели идентичные размеры.
- Для удобства чтения сохраненный данных добавил сохранение в формате JSON. Благо есть готовая либа, которая превращает форматирование, сохранение и чтение в сплошное удовольствие.
- Пришлось учесть человеческий фактор. Бывает, что запустишь скрипт на пробу, а он при сохранении результата затирает предыдущий файлик, сделанный с особой любовью и тщательностью. Поэтому результат настройки сохраняется в текущую директорию, а не в рабочую подпапку ./src Для дальнейшей работы нужно ручками перенести его в требуемое место. После этого случайные затирания у меня не повторялись. Файлик имеет имя вида pf_1280_720.txt – цифры разрешения подставляются автоматически исходя из того, что задано в начале скрипта.
- Если вы хотите запускать скрипт на малине без камеры или на десктопе, в коде нужно указать путь к картинке. В этом случае скрипт не грузит библиотеки работы с камерой и можно работать. В коде для этих целей оставлены строчки:
loadImagePath = "" # loadImagePath = "./src/scene_1280x720_1.png"
Скрипт третий — серия фото для калибровки
Фундаментальная наука говорит, что для успешного построения карты глубин стереопара должна быть откалибрована. А именно, все ключевые точки с левой картинки должны находиться на той-же высоте и на правой картинке. При таком раскладе функция StereoBM, которая у нас единственная real-time, может успешно делать свое дело.
Для калибровки нам нужно напечатать эталонную картинку, сделать серию фотографий и отдать её алгоритму калибровки, который высчитает все искажения и сохранит параметры для приведения картинок в норму.
Итак, печатаем «шахматную доску» и приклеиваем на твердую плоскую поверхность. Для простоты серийного фото был сделан скрипт с таймером обратного отсчета, который выводится поверх видео.
Вот как выглядит скрипт серийного фотографирования в работе:
Что интересного в коде 3-го скрипта
- Исходный код третьего скрипта
- Был словлен неприятный сюрприз. Оказалось, что картинка на предпросмотре и захваченное фото сильно отличаются. То, что было видно на превьюшке, оказывается частично «отрезано» на фото. Причина в том, что захват малиновой камерой фото и видео сильно отличается – как по работе с самим сенсором, так и по постобработке результата. Поэтому далее везде при захвате картинки мы принудим функцию camera.capture () использовать видеопоток, указав параметр use_video_port=True.
- В скрипте использован вывод текста поверх видео – с помощью camera.annotate_text() оказалось очень удобно показывать таймер обратного отсчета перед следующим фото. Экспериментально был подобран период 5 секунд – за это время можно спокойно разместить шахматную доску в новом положении.
- Опять-же, для борьбы со случайным затиранием предыдущей серии все фото сохраняются в текущую директорию, и для дальнейшей работы их надо перенести в папку ./src
Надо особо отметить, что «правильность» сделанной серии критична для результатов калибровки. Чуть позже мы посмотрим на результат, который получается при неверно сделанных фотографиях.
Скрипт 4 — резка фото на стереопары
После того как серия фото сделана, сделаем еще один сервисный скриптик, который берет всю сделанную серию фото и режет её на пары картинок – левые и правые, и сохраняет пары в папку ./pairs Просмотр нарезанных пар позволяет оценить, насколько хорошо мы настроили удаление расфокусированной зоны по центру картинки. Скрипт достаточно зауряден, поэтому видео я спрятал под спойлер.
Пример работы и ссылка на исходники 4-го скрипта
- Исходники 4-го скрипта
- Видео с работой четвертого скрипта:
Самое интересное — калибровка, скрипт пятый
Скрипт калибровки скармливает все стереопары из папки ./src в функцию калибровки и погружается в раздумье. После своей непростой работы (для 15 картинок 1280х720 на первой малине это около 5 минут) он берет последнюю стереопару, «исправляет» картинки (ректифицирует) и показывает уже исправленные версии, по которым можно строить карту глубин.
Вот как выглядит скрипт в работе:
Что интересного в коде 5-го скрипта
- Исходники пятого скрипта
- Для упрощения работы использована библиотека StereoVision. Правда, в оригинальной версии она вываливалась с ошибкой, если не находила шахматную доску на картинке, поэтому я сделал свой форк и добавил игнорирование «плохих» стереопар.
- По умолчанию скрипт по очереди показывает каждую картинку с отмеченными на ней найденными точками на шахматной доске, и ждет нажатие клавиши пользователем. Это полезно при отладке. Если вы хотите включить «тихий» режим, не требующий вмешательства юзера, то в строчке
замените True на False – картинки показываться не будут, кроме последней откалиброванной пары.calibrator.add_corners((imgLeft, imgRight), True)
«Что-то пошло не так»
Бывают случаи, когда результаты калибровки оказываются неожиданными.
Вот парочка ярких примеров:
На самом деле калибровка – определяющий момент. От его качества напрямую зависит то, что мы получим на этапе построения карты глубин. После большого количества экспериментов вырисовался такой список требований к съёмке:
- Картинка с шахматами не должна быть параллельна плоскости фото – обязательно под разными углами. Но и без фанатизма — если доску держать почти перпендикулярно плоскости фото, то скрипт просто не найдет шахматы на картинке.
- Свет, хороший свет. При слабом комнатном освещении, да еще и при выдергивании картинок из видео, падает качество картинки. В моем случае свет в 90% случаев сразу исправлял ситуацию.
- Интернеты пишут, что доска должна по возможности занимать максимальное место на изображении. Действительно помогает.
Вот как выглядит «исправленная» стереопара при хороших результатах калибровки:
Скрипт 6 — первая попытка построить карту глубин
Типа все готово – можно уже и карту глубин построить. Загружаем результаты калибровки, делаем фото и смело строим карту глубин с помощью cv2.StereoBM
Получаем примерно следующее:
Результат не очень впечатляет, явно надо что-то подкручивать. Ну что-же, приступим к более тонкой настройке в следующем, 7-м скрипте. Там мы будем использовать для построения не 2 параметра, как в StereoBM(), а почти 10, что гораздо интереснее.
Вот исходники 6-го скрипта
Скрипт 7 — карта глубин с расширенными настройками
Когда параметров не 2 а 10, то перебирать их варианты с постоянным перезапуском скриптов занятие неправильное. Поэтому был сделан скрипт для удобной интерактивной настройки карты глубин. Задача стояла не усложнять код работой с интерфейсом, поэтому все было сделано на matplotlib. Прорисовка интерфейса в matplotlib на малине происходит достаточно медленно, поэтому я обычно переношу рабочую папку с малины на ноутбук и подбираю параметры там. Вот как выглядит работа скрипта:
После того, как вы подобрали параметры, скрипт по кнопке Save сохраняет результат в файлик 3dmap_set.txt в формате JSON.
Что интересного в коде 7-го скрипта
- Исходнички 7-го скрипта
- Очень большую часть кода занимает работа с интерфейсом. Собственно, к каждому слайдеру-бегунку привязано событие изменения его значения, которое приводит к перестроению и перерисовке карты глубин.
- Важный момент в том, что на многие параметры карты глубин наложены определенные ограничения. Например, numOfDisparities должно быть кратно 16, а три других параметра должны быть нечетными числами не менее определенного значения. Поэтому после обновления юзером эти значения приводятся к правильным функцией update(val), но на самом интерфейсе справа от слайдеров могут отображаться значения самого слайдера. Например при numOfDisparities на слайдере равном 65.57 реальное используемое значение это 64. Я не стал отдавать приведенные значения назад на слайдер, так как это приводит к повторной перерисовке карты глубин и занимает время.
- В качестве бонуса при рисовании с matplotlib у нас сразу получается цветная карта глубин, её не требуется дополнительно раскрашивать.
Практическая работа с картой глубин показала, что в первую очередь нужно подбирать параметр minDisparity, и в связке с ним numOfDisparities. Ну и помним, что numOfDisparities меняется на самом деле дискретно, с шагом 16.
После настройки этой пары можно поиграться с другими параметрами.
Особенности настройки карты – это уже дело вкуса пользователя, и зависит от решаемой задачи. Можно привести карту к большому количеству мелких деталей либо выводить укрупненные зоны. Для простого обхода препятствия роботами второе подходит больше. Для облака точек первое, но тут всплывают вопросы производительности (мы к ним еще вернемся).
Что хотим увидеть?
Ну и пожалуй один из самых важных моментов — это юстировка «дальнозоркости» нашего девайса. При настройке карты глубин я обычно располагаю один объект на расстоянии около 30 см, второй — в метре, и остальные в двух метрах и далее. И при настройке в 7-м скрипте первых двух параметров (minDisparity и numOfDisparities) добиваюсь следующего:
- Ближайший объект (в 30 см) — красный цвет
- Объект в полуметре-метре — желтый или зеленый
- Объекты в 2-3 метрах — зеленые или светло-синие
В итоге получаем систему, настроенную на распознавание «ближней» зоны препятствий в радиусе до 5-10 метров.
Работаем с видео на лету — скрипт 8-й, заключительный
Ну что, теперь у нас имеется готовая настроенная система, и надо бы получить практический результат. Пробуем строить карту глубин в реальном времени по видео с нашей камеры, и показывать её в реальном времени по мере обновления.
Что интересного в коде 8-го скрипта
Первый вопрос – куда рисовать карту. Самый быстрый вариант, который пришел мне в голову, это отображение карты поверх видео в слое overlay. Для этого пришлось решить несколько проблем, а именно:
- Overlay представляет из себя массив в три слоя R, G и B, а карта у нас изначально черно-белая в одном слое. Это решается раскраской grayscale в цвета строчкой
disparity_color = cv2.applyColorMap(disparity_grayscale, cv2.COLORMAP_JET)- Цвета в overlay идут в последовательности RGB, а раскрашенная картинка в BGR – приходится на лету менять цвета местами функцией cv2.cvtColor()
- Overlay должен иметь размеры массива кратные 16. Если разрешение не кратно 16 — приводим к нужному ручками.
В гонке за скоростью
Итак, первый замер делался на первой Raspberry с одноядерным процессором.
— 4 секунды — построение карты по изображению 1280х720 Это много.
— 2,5 секунды — на Raspberry Pi 2, уже лучше.
Анализ показал, что при этом на второй малине используется всего одно ядро. Непорядок! Я пересобрал OpenCV с использованием библиотеки распараллеливания TBB.
— 1,5 секунды – запуск на второй малине с использованием многоядерности. По факту оказалось, что используются всего 2 ядра – с этим предстоит еще повозиться. Оказалось, что на эту проблему наткнулся не только я, значит еще есть куда двигаться.
Судя по алгоритму, скорость работы должна линейно зависеть от размера обрабатываемых данных. Поэтому, если снизить разрешение в 2 раза, то теоретически все должно работать в 4 раза быстрее.
— 0,3 секунды, или примерно 3-4 FPS – при сниженном вдвое разрешении 640х360. Теория подтвердилась.
Дальнейшие планы
В первую очередь я хотел выжать максимум из многоядерности второй малины. Посмотрю подробнее исходники функции StereoBM и попробую понять, почему работа идет не на всю катушку.
Следующий этап обещает гораздо больше приключений — это использование GPU малины для ускорения расчетов.
Тут рисуются три возможных пути:
- Есть успешный пример построения карты глубин с помощью GPU, в котором удалось достичь 10 FPS, причем дело было на первой малине. Я списался с автором — он говорит, что код был написан «с нуля» без использования OpenCV, и написал некоторые рекомендации.
- Также интересный подход был обнаружен у японского товарища Koichi Nakamura, который пишет под GPU на ассемблере под питоном, вот его наработки на гитхабе. Он ответил, что может попробует сделать что-то для OpenCV, но не в ближайшее время.
- Ну и запрос в фейсбучной англоязычной группе малинщиков дал мне ссылку на интересную книжку Jan Newmarch "Programming AudioVideo on the Raspberry Pi GPU"
Если у вас был опыт по работе с TBB под малиновое OpenCV, или вы имели дело с кодингом под GPU малинки — буду благодарен за дополнительные подсказки-наводки. Готовых наработок мне удалось найти достаточно мало по одной простой причине — малина с двумя камерами явление редкое. Если цеплять две вебки по USB то наступают большие тормоза, а с двумя родными камерами умеет работать только Raspberry Pi Compute, к которой нужна еще здоровенная devboard со шнурками и переходниками.
Полезные ссылки:
Рабочие скрипты:
- Исходники всех 8 скриптов на гитхабе
- Образ карточки (2,6Гб в архиве)
Настройка OpenCV и Python на малине:
- Отличный блог «питониста-OpenCV-шника» и мануал по настройке Python и OpenCV на малине
Библиотека StereoVision:
- Мой рабочий форк на гитхабе
- Оригинал от Egret на гитхабе
- Описание автора StereoVision в его блоге
Работа с GPU
- Либы японца Koichi Nakamura на гитхабе — ассемблер под питон для GPU
- Книжка Jan Newmarch "Programming AudioVideo on the Raspberry Pi GPU"
- Если нужно работать с камерой малины Like a Boss:
Picamera документация
Ну и интересная статья на хабре про «Чтобы распознавать картинки, не нужно распознавать картинки»
Комментарии (21)
schroeder
21.12.2015 14:39Можно по-подробней про зеркала? Схема расположения, размеры?
Realizator
21.12.2015 15:07+3Считалось в матлабе с кучей пограничных условий. Если получится упростить чертежи и сделать их повторяемыми без заморочек с вакуумным напылением, выложу все чертежи как OpenSource. Думаю в течение месяца-двух закончу.
schroeder
21.12.2015 14:42для двух камер я использовал мультиплексер. Правда время переключения камер большое, порядка 300мс.
Realizator
21.12.2015 15:10По вашей ссылке ценник 240TL — я так понял это Турецкая Лира? Получается около 5 000 рублей? Это как малина вместе с камерой примерно.
Ну и касательно 300 мс — да, сильно не разгонишься. У меня сейчас примерно 300-400мс выходит, но это еще не в полную силу многоядерность малинки использована. Думаю 100 мс вполне реально выжать, даже без GPU.
BelBES
21.12.2015 15:15+4Выглядит очень даже круто, сам подумывал о системе зеркал для стереокамеры, чтобы с синхронизацией кадров не морочиться. Но что у вашей камеры с baseline? Я так понимаю он тут не больше 5-7 см, т.е. бесконечность у камеры начинается где-то в паре тройке метров перед ней?
Realizator
21.12.2015 15:17Стереобаза в районе полутора-двух сантиметров. На расстоянии метра в 4-5 еще дает диверсификацию по дистанции. Текущий вариант самый маленький из тех что я пробовал — для задач малых дистанций самое то. Если базу увеличивать — вся конструкция резко вырастает в размерах и становится больше самой малины. Текущие размеры — 40х45х54 мм.
alexbuyval
22.12.2015 14:41А сколько времени примерно занимает получение кадра с камеры в память малинки? Насколько я понял 300 мс это общее время?
Realizator
22.12.2015 15:47Ну я даже не замерял этот параметр. Камера может спокойно отдавать HD или FullHD под 30 кадров в секунду. Основной поедатель времени — это само построение карты глубин.
alexbuyval
22.12.2015 16:04Ясно. Спасибо. Я просто пытался получать кадры стандартными узлами ROS и у меня выходила задержка почти в 400 мс. Я собственно и решил, что транспорт видео кадров в ROS между узлами слишком ресурсоемкий.
Realizator
22.12.2015 16:22А ROS у вас на борту малины стоял получается? Я с ним дела не имел, но есть желание попробовать.
alexbuyval
22.12.2015 16:28Да, на малине (версия для Ubuntu ARM). Я пришел к выводу, что для обработки видео в ROS на arm процессорах, нужно либо делать один узел, который и получает данные с камеры и он же обрабатывает их, либо попытаться использовать технологию nodelet, которая вместо передачи сообщений между узлами копирует только указатели на данные этих сообщение. Т.е. нужно избегать передачи видео сообщений между узлами ROS.
TimID
23.12.2015 01:21А может лучше попробовать запустить отдельным потоком приложение для расчёта карты глубины, а потом данные перебрасывать через какой-нибудь сокет как 320х240х16? Так ROS сможет обрабатывать данные?
alexbuyval
23.12.2015 08:32Да, так можно. Но по-сути узел ROS тоже отдельный поток, поэтому если там сразу будет и захват видео и обработка, то получиться тоже самое, только со стандартным транспортом сообщений.
TimID
23.12.2015 09:16Я предлагаю схитрить, просто. Для расчёта карты глубины нужно как можно более разрешение, а для целей навигации сама карта глубины может быть и 320х240.
Если сделать «нативное» приложение по генерации карты, может быть будет быстрее?alexbuyval
23.12.2015 09:24Да, я понял. На самом деле узел ROS это тоже нативное приложение, т.к. ROS — это просто фреймфорк, а не OS.
Meklon
Насчет зеркал. Можно взять стоматологические зеркала с родиевым покрытием.
Примерно 150 рублей за штуку.
Без двойных отражений и с очень высокой четкости.
Realizator
Мысль интересная. Только чем вот их резать? Там основа — обычное стекло?
IronHead
А блины от жестких дисков не подойдут? Они отражают наружным слоем, режутся легко обычным дремелем (они из алюминия), достаются тоже легко, нужно только брать жесткие диски по старее. Я из таких дисков делал зеркала для отклонения лазерного луча.
Meklon
Да, стекло. И требования очень высокие к гладкости и коэффициенту отражения. Слишком мелкая работа у стоматологов. Резать, вероятно алмазными борами из того же магазина, сошлифовывая лишнее.
Realizator
Подход интересный, но больно уж сложный. У меня форма зеркал сложная — они одновременно являются элементами жесткости всей конструкции. Я хотел все упростить, сейчас есть еще несколько идей — буду в ближайшее время пробовать.
Meklon
Возможно) Просто предложил источник хороших зеркал.