Сведение двух камер Orbbec в единое облако точек

Одна камера глубины всегда видит сцену с одной точки: за спинами людей остаются тени, блики дают дырки, стеллаж закрывает проход. Классическое решение — поставить две камеры Orbbec с разных ракурсов и свести их облака точек в общий базис. Задача распадается на три части: открыть два устройства в одном процессе, найти внешнюю калибровку (матрицу поворота и вектор переноса между системами координат камер) и применить её к точкам. В этом гайде соберём рабочую склейку на Python с pyorbbecsdk и Open3D, разберём, откуда взять калибровку и почему без синхронизации движущиеся объекты «размажутся» в двух слоях. За основу возьмём Femto Bolt — iToF-камеру с диапазоном 0,1–5,5 м и RGB 4K, но механика одинакова для любой пары Orbbec.

Где это применяется чаще всего: обкат робота перед препятствиями (две камеры закрывают переднюю полусферу без теневых зон), контроль стеллажей и проёмов, AR-стенды, где человек ходит по площадке, и любые инспекционные посты, где объект крупный и его нельзя обойти одним взглядом. Вторая камера — это ещё и избыточность: в момент, когда первая камера ловит блик от стекла, вторая видит ту же зону под другим углом. Заранее разделите эти два мотива — «покрытие» и «резервирование»: от них зависят и расстановка, и требования к синхронизации.

Что понадобится

• две камеры Orbbec (пример на Femto Bolt) и хост с двумя контроллерами USB 3.x;
• Python 3.8+, pyorbbecsdk, NumPy, Open3D, OpenCV;
• шахматная мишень (калибровочная доска) для внешней калибровки;
• серийные номера обеих камер (посмотрите их примером-«device list» из комплекта pyorbbecsdk);
• желательно — кабель синхронизации, если камеры будут видеть движущиеся объекты.

Пошаговая настройка

Шаг 1. Открываем оба устройства

Каждую камеру открываем своим Pipeline с указанием серийного номера — так порядок перечисления USB-устройств не влияет на логику:

import numpy as np
from pyorbbecsdk import Pipeline, Config

SERIAL_A = "AXXXXXXXXX1"   # впишите свои серийники
SERIAL_B = "AXXXXXXXXX2"

def open_camera(serial: str) -> Pipeline:
    config = Config()
    config.set_serial_number(serial)  # сверьте с examples вашей версии SDK
    pipeline = Pipeline(config)
    return pipeline

pipeline_a = open_camera(SERIAL_A)
pipeline_b = open_camera(SERIAL_B)
pipeline_a.start()
pipeline_b.start()

def grab_depth(pipeline) -> np.ndarray:
    frames = pipeline.wait_for_frames(100)
    if frames is None:
        raise TimeoutError("no frameset")
    depth_frame = frames.get_depth_frame()
    h, w = depth_frame.get_height(), depth_frame.get_width()
    depth = np.asarray(depth_frame.get_data(), dtype=np.uint16).reshape(h, w)
    return depth

Если конструктор с серийником в вашей сборке называется иначе, посмотрите примеры из комплекта — в pyorbbecsdk есть готовые скрипты работы с двумя устройствами. Главная мысль не меняется: два независимых Pipeline, каждый со своим устройством.

Шаг 2. Снимаем внешнюю калибровку

Матрица преобразования связывает системы координат двух камер. Практичный способ её получить: расставьте камеры, положите в зону обзора шахматную мишень, снимите по кадру RGB с каждой и решите задачу cv2.calibrateCamera + оценка позы доски (solvePnP) для каждой камеры. Поза доски относительно камеры A и относительно камеры B даёт цепочку: T_A->доска и T_B->доска, отсюда T_B->A = T_A->доска * inv(T_B->доска). Снимите 10–15 поз мишени по всей рабочей зоне и усредните — одна поза даёт шум в градусы. Для грубых задач допустима ручная подгонка матрицы с проверкой по облаку, но экономит она полчаса, а потом отнимает дни.

Пара советов по мишеням. Классическая шахматная доска хорошо работает, но печатайте её на жёсткой подложке — бумага, приклеенная к картону, «гуляет» на полградуса, и это ровно ваша целевая точность. Априко-маркеры (ArUco/AprilTag) удобнее в темноте и при дальних дистанциях: маркеры распознаются по одиночке, и мишень можно растянуть на большую площадь. Фиксируйте и возвращайте мишень в рабочую зону: калибровка, снятая «в воздухе» перед камерами, не гарантирует точности в объёме сцены, где реально будут объекты.

# T_ab: 4x4, переводит точку из базиса B в базис A (сверьте порядок с вашей конвенцией)
T_ab = np.array([
    [ 0.9999, -0.0050,  0.0100, -0.850],
    [ 0.0050,  0.9998,  0.0200,  0.030],
    [-0.0100, -0.0200,  0.9997,  0.120],
    [ 0.0,     0.0,     0.0,    1.0  ],
], dtype=np.float64)

Шаг 3. Переводим глубину в точки

Каждый валидный пиксель глубины разворачивается в 3D-точку через интринсики depth-камеры. Возьмите fx, fy, cx, cy из camera_info или средств SDK:

def depth_to_points(depth: np.ndarray, fx: float, fy: float,
                    cx: float, cy: float) -> np.ndarray:
    h, w = depth.shape
    u = np.arange(w)[None, :].repeat(h, 0).astype(np.float32)
    v = np.arange(h)[:, None].repeat(w, 1).astype(np.float32)
    valid = depth > 0
    z = depth.astype(np.float32) / 1000.0   # миллиметры -> метры
    x = (u - cx) * z / fx
    y = (v - cy) * z / fy
    pts = np.stack([x, y, z], axis=-1)[valid]
    return pts

def transform_points(pts: np.ndarray, T: np.ndarray) -> np.ndarray:
    ones = np.ones((pts.shape[0], 1), dtype=pts.dtype)
    hom = np.hstack([pts, ones])
    return (T @ hom.T).T[:, :3]

Шаг 4. Склеиваем и чистим облако

Складываем оба облака (каждое — в общем базисе) и прореживаем воксельным фильтром, иначе файлы и визуализация раздуваются:

import open3d as o3d

pts_a = transform_points(depth_to_points(grab_depth(pipeline_a), FX, FY, CX, CY), np.eye(4))
pts_b = transform_points(depth_to_points(grab_depth(pipeline_b), FX2, FY2, CX2, CY2), T_ab)

merged = np.vstack([pts_a, pts_b])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(merged)
pcd = pcd.voxel_down_sample(voxel_size=0.01)   # 1 см
o3d.visualization.draw_geometries([pcd])

Критерий качества склейки — швы. Наведите облако на плоскость пола: если после слияния пол один и толстый, калибровка хорошая; если пола два слоя со смещением в сантиметры, переснимайте калибровку. Уточнить результат можно ICP в Open3D (registration_icp) поверх стартовой оценки — он доводит остаточные градусы, но не спасёт от грубой ошибки.

Шаг 5. Проверяем качество склейки числами

Глаз обманчив, поэтому добавьте автоматическую метрику. Самая практичная — расстояние до известной плоскости: снимите сцену с ровным полом, постройте плоскость RANSAC по каждой камере отдельно, преобразуйте облако B в базис A и измерьте среднее расстояние точек B до плоскости A. Несколько миллиметров — отличная калибровка, сантиметры — рабочая для навигации, больше — переснимайте. Метрика повторяема, её можно выводить в лог после каждой перекалибровки, и тогда деградация монтажа (крепление ослабло, камеру задели) видна по числам, а не по жалобам.

Разбор кода

Почему облака считаются отдельно, а не одним вызовом? Потому что у каждой камеры свои интринсики и своё положение, и «склейка» — это по определению две проекции в общий базис. Мы сознательно держим depth_to_points в чистом NumPy: на 640-кратных матрицах это миллисекунды, зато формула прозрачна и легко проверяется (центральный пиксель при калиброванных cx, cy обязан дать x≈0, y≈0). Воксель в 1 см — компромисс: при 5 мм облако двойной камеры весит сотни мегабайт в секунду, при 2 см теряется мелкая геометрия. Если камеры снимают статику, кадры можно усреднять по глубине — шум ToF заметно падает. Для движущихся объектов добавляется жёсткое требование: кадры обеих камер должны быть сняты в один момент, иначе человек у вас будет двоиться. Это решается аппаратной синхронизацией — у Orbbec она есть, и мы разобрали её отдельно в гайде о синхронизации нескольких Femto.

Если склейка нужна постоянно и в реальном времени, переносите её в ROS 2: запускаете два экземпляра драйвера orbbec_camera в разных namespace, публикуете static_transform_publisher с калибровкой между фреймами и складываете PointCloud2 уже с TF-преобразованием. Логика та же, но трансформации берёт на себя TF.

Пара слов о производительности и структуре данных. Облако, которое мы собираем в NumPy, — неорганизованное: список точек без привязки к пиксельной сетке. Это нормально для слияния и визуализации, но помните, что и исходные depth-кадры остаются организованными матрицами — большинство масок и ROI удобнее считать в 2D, а разворачивать в точки уже после фильтрации: так вы не платите преобразованием за пиксели, которые всё равно отбросите. Воксельный фильтр ставьте как можно раньше в цепочке — все последующие операции линейны по числу точек, и прореживание на входе дешевле, чем где-либо ещё.

Схема	Когда выбирать	Сложность
Одна камера с широким FOV	статика, небольшие слепые зоны допустимы	минимальная
Две камеры + программная склейка	статичные сцены, контроль стоимости	калибровка раз, дальше автоматика
Две камеры + аппаратная синхронизация	люди, конвейер, любое движение	калибровка + sync-кабели
GMSL2-стереокамера в движении	мобильные роботы, длинные шлейфы	заводское решение

Из таблицы видно главное: пока сцена статична, программной склейки достаточно, и усложнять не нужно. Требование синхронности появляется ровно в момент, когда в кадре появляется движение, — и это же момент, когда стоит проверить, не закрывает ли одну камеру вторая: расстановка, выгодная по покрытию, не всегда выгодна по взаимным теням.

Частые проблемы

Вторая камера не открывается

Проверьте топологию USB: lsusb -t должно показать камеры на разных контроллерах, каждая на 5000M. Две камеры на одном корневом концентраторе не поделят полосу — увидите обрывы и таймауты wait_for_frames. Вторая частая причина — в конфиге обеих Pipeline указан один и тот же серийник: SDK тогда дважды открывает первое найденное устройство или падает с «device busy». Логи при старте Pipeline печатают открытый серийник — сверьте его.

Пол двойным слоем, швы на стенах

Это ошибка внешней калибровки, и обычно — вырожденная мишень. Если все позы доски были в одной плоскости, перенос определяется плохо. Переснимайте калибровку с мишенью в разных наклонах и глубинах, усредняйте не меньше десятка поз, затем проверяйте по плоскости пола — она самый честный «тестер». Небольшой систематический сдвиг добивается ICP, но градусы там не вылечить.

Камеры разных моделей: как склеивать

Ничто не мешает свести в один базис Femto Bolt и Gemini — матрица преобразования и интринсики у каждой свои, остальная схема идентична. Что действительно различается, так это характер ошибок: ToF и стерео по-разному ведут себя на краях объектов и на слабо текстурированных поверхностях, поэтому на стыках зон вы увидите разные «почерки» шума. Практический вывод: зоны обзора разводите так, чтобы один объект целиком снимала одна камера, и стык проходил по пустым областям — тогда разница технологий останется невидимой для итогового облака.

Двоение движущихся объектов

Кадры сняты в разные моменты. Программный старт двух Pipeline не даёт гарантии одновременности — расхождение в десятки миллисекунд на шагающем человеке даёт сантиметры. Нужна аппаратная синхронизация камер по кабелю или переход на решение, где синхронность заложена в архитектуру. Альтернатива для слабых требований — снимать сцену, когда движение остановилось, но это редко применимо.

Компоненты для такой сборки есть в каталоге Orbbec на luwam.ru. Как основные камеры для сцепки берут Femto Bolt (iToF 0,1–5,5 м, RGB 4K) и Gemini 335L; для мобильных платформ посмотрите также GMSL2-варианты линейки Gemini. Поможем с конфигурацией и консультацией по SDK, выставим счёт для юрлица, гарантия 1 год.