Проект «робот-грузчик»: определение ориентации

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

Я обсудил свои затруднения с девушкой-гуманитарием, и спросил, какие ей известны способы ориентации в пространстве. По её словам, в Лондонском музее науки она застала экспозицию, посвящённую ориентации муравьёв по виду вертикально вверх над головой. Посетителям предлагалось взять зеркало и идти по комнате, разглядывая в это зеркало узоры на потолке и ориентируясь лишь по ним. (Карта потолка прилагалась.)

Я решил проверить: что видит на потолке склада мой робот?


Даже на такой низкокачественной записи отлично различимы длинные прямоугольные окна. Все они, естественно, параллельны одной из осей склада, а значит, параллельны и рядам полок. Получается, если «в кадр» попадёт окно и я смогу определить его направление — то я получаю и направление робота, с точностью до 180°. Окна видно не из любой точки склада, но для периодической коррекции курса — они попадают в кадр достаточно часто.

В распознавании образов я не мастак, и первым делом я спросил на Q&A — нет ли для распознавания прямоугольных окон чего-нибудь уже готового, например, применим ли OpenCV? К сожалению, ничего толкового мне не подсказали — люди, которые «в теме», сочли ниже своего достоинства разжёвывать чайнику азы.

Американский форум: задал вопрос — получишь ответ.
Израильский форум: задал вопрос — получишь вопрос.
Русский форум: задал вопрос — тебе объяснят, какой ты мудак.

Тем более, что робот управлялся из-под Microsoft Robotics Development Studio, а готовой .NET-сборки для работы с OpenCV я не нашёл — я решил писать собственное распознавание с нуля. Не ракетная наука, чай — всего-то распознавать ярко-белые прямоугольники.

Потолок склада роботом видится примерно так:


Для начала отделим ярко-белое окно от тёмного фона. Переводим из RGB в YCbCr и разделяем по Y=227 (порог выбран опытным путём, и в идеальном мире подбирался бы адаптивно по яркости изображения в целом). Попутно уменьшаем исходное изображение 640х480 до 320х240 — нам этого достаточно, а обработка ускорится вчетверо.

Код:

byte[] bytes = response.Frame;
int stride = bytes.Length / height;

byte[,] belong = (byte[,])Array.CreateInstance(typeof(byte), new int[] { 326, 246 }, new int[] { -3, -3 });

int Ythreshold = settings.Ythreshold;
for (int y = 0; y < 240; y++)
{
    int offset = stride * y * 2;
    for (int x = 0; x < 320; x++)
    {
        int blu = bytes[offset] + bytes[offset + 3] + bytes[offset + stride] + bytes[offset + stride + 3]; offset++;
        int grn = bytes[offset] + bytes[offset + 3] + bytes[offset + stride] + bytes[offset + stride + 3]; offset++;
        int red = bytes[offset] + bytes[offset + 3] + bytes[offset + stride] + bytes[offset + stride + 3]; offset += 4;

        belong[x, y] = (0.299 / 4 * red + 0.587 / 4 * grn + 0.114 / 4 * blu > Ythreshold ? (byte)1 : (byte)0);
    }
}

В результате разделения получается такое изображение:


Как обычно на видеоизображениях, впридачу к правильно выделенному окну мы получили шум из отдельных пикселов на бликующих поверхностях, да заодно внутри самого окна оказались «выбитые пиксели», недостаточно светлые.

Удаляем шум медианным фильтром (хотя в QA мне отчего-то советовали гауссовское размытие. Ну вот зачем здесь размытие?) с радиусом 3 пиксела, и порогом в 5 светлых пикселей из 21 рассмотренных. Такой фильтр «склоняется» к соединению светлых областей, между которыми есть тёмные пиксели — так, чтобы наше окно из трёх стёкол объединилось в один прямоугольник.

private bool[,] filtered = (bool[,])Array.CreateInstance(typeof(bool), new int[] { 326, 246 }, new int[] { -3, -3 });

int medianThreshold = settings.MedianThreshold;
for (int x = 0; x < 320; x++)
    for (int y = 0; y < 240; y++)
              filtered[x, y] = belong[x - 1, y - 2] + belong[x, y - 2] + belong[x + 1, y - 2] +
        belong[x - 2, y - 1] + belong[x - 1, y - 1] + belong[x, y - 1] + belong[x + 1, y - 1] + belong[x + 2, y - 1] +
        belong[x - 2, y * 1] + belong[x - 1, y * 1] + belong[x, y * 1] + belong[x + 1, y * 1] + belong[x + 2, y * 1] +
        belong[x - 2, y + 1] + belong[x - 1, y + 1] + belong[x, y + 1] + belong[x + 1, y + 1] + belong[x + 2, y + 1] +
                               belong[x - 1, y + 2] + belong[x, y + 2] + belong[x + 1, y + 2] > medianThreshold;

Размерность для массивов belong и filtered — [-3..322, -3..242] — выбрана нарочно с «полями» по три пиксела с каждой стороны, чтобы работать с этими массивами, не заморачиваясь проверками индексов.

После фильтрации на изображении остаются белыми только окно, объединённое из трёх стёкол, и несколько бликов на полке:


Постановим, что самое большое белое пятно в кадре — это непременно окно. Зальём (floodfill) каждое белое пятно, и возьмём наибольшее по площади.

int biggest_area = 0;
byte area_id = 1, biggest_id = 0; // areas start from 2
Rectangle bounds = new Rectangle();
PointF cg = new PointF(); // center
Point[] stack = new Point[320*200]; 

for (int x = 0; x < 320; x++)
    for (int y = 0; y < 240; y++)
        if (filtered[x, y] && belong[x, y] <= 1)
        {
            int area = 0, left = 320, top = 240, right = 0, bottom = 0;
            int sx = 0, sy = 0;
            ++area_id;
            // FloodFill 
            int sp = 0;
            stack[0] = new Point(x, y);
            while (sp >= 0)
            {
                Point next = stack[sp--];
                area++;
                sx += next.X;
                sy += next.Y;
                belong[next.X, next.Y] = area_id;

                if (next.X < left) left = next.X;
                if (next.X > right) right = next.X;
                if (next.Y < top) top = next.Y;
                if (next.Y > bottom) bottom = next.Y;

                if (filtered[next.X - 1, next.Y] && belong[next.X - 1, next.Y] <= 1) stack[++sp] = new Point(next.X - 1, next.Y);
                if (filtered[next.X, next.Y - 1] && belong[next.X, next.Y - 1] <= 1) stack[++sp] = new Point(next.X, next.Y - 1);
                if (filtered[next.X, next.Y + 1] && belong[next.X, next.Y + 1] <= 1) stack[++sp] = new Point(next.X, next.Y + 1);
                if (filtered[next.X + 1, next.Y] && belong[next.X + 1, next.Y] <= 1) stack[++sp] = new Point(next.X + 1, next.Y);
            }
            if (area > biggest_area)
            {
                biggest_area = area;
                biggest_id = area_id;
                bounds = new Rectangle(left, top, right - left, bottom - top);
                cg = new PointF((float)sx / area, (float)sy / area);
            }
        }

Изображение для распознавания готово! Двухуровневое — белый прямоугольник на чёрном фоне. Мы даже вычислили во время заливки координаты его «центра масс» cg (на изображениии — красная точка) и границ (зелёная точка — центр bounding box). Можем теперь проверить, насколько наше белое пятно похоже на окно: площадь biggest_area должна быть не меньше 2000 пикселей, расстояние между двумя центрами — не больше 20 пикселей, иначе фигура слишком несимметрична для прямоугольника. Но как дальше будем определять его ориентацию?


Ракетные учёные, может быть, применили бы преобразование Хафа, перевели бы изображение в 4-мерное пространство вероятностных параметров прямоугольника (ширина, длина, угол наклона, смещение от начала координат), и искали бы там максимум. Но я подошёл к задаче по рабоче-крестьянски, и для начала составил «гистограмму» удаления точек прямоугольника от его геометрического центра:

PointF c = new PointF(bounds.Left + (float)bounds.Width / 2, bounds.Top + (float)bounds.Height / 2);

int[] hist = new int[400];

for (int i = 0; i < 400; i++) hist[i] = 0;

int maxdist = 0;
for (int x = bounds.Left; x <= bounds.Right; x++)
    for (int y = bounds.Top; y <= bounds.Bottom; y++)
        if (belong[x, y] == biggest_id)
        {
            int dist = (int)Math.Sqrt(Sqr(x - c.X) + Sqr(y - c.Y));
            hist[dist]++;
            if (dist > maxdist) maxdist = dist;
        }

Серый график — это и есть гистограмма, на ней тёмная полоса — максимум, т.е. наибольшее число точек находится именно на таком расстоянии от центра прямоугольника. (На прямоугольнике проведена тёмная окружность, соответствующая этому расстоянию.) Легко понять, что это расстояние как раз и есть полуширина прямоугольника: до неё — окружности целиком помещаются внутри прямоугольника, и гистограмма линейно (с коэффициентом π) растёт; потом гистограмма медленно убывает, пока не дойдёт до полудлины прямоугольника; с этого момента внутри прямоугольника помещаются только четыре «уголка» окружностей, и гистограмма резко спадает. К сожалению, найти длину через этот «обрыв» на гистограмме не удаётся — края прямоугольника оказываются слишком рваными, и зашумляют конец гистограммы. Мы пойдём другим путём, и рассмотрим круг на 5 пикселей шире прямоугольника. В нём будут две чёрные «боковушки» как раз на поперечной оси прямоугольника:



Аккуратно найдём центры масс этих «боковушек»: сложность здесь в том, чтобы их разделить. Считаем отдельно центр масс в каждом «квадранте», потом объединяем «квадранты» в две пары по близости центров.

int r1 = 0; // incircle radius
for (int x = maxdist; x >= 3; x--)
{
    if (hist[x] > hist[r1]) r1 = x;
}

int rSample = r1 + 5;
int[] voters = new int[4];
Point[] sums = new Point[4];
Point sampleOrg = new Point(Math.Max((int)(c.X - rSample), 0),
                            Math.Max((int)(c.Y - rSample), 0));
Rectangle sample = new Rectangle(sampleOrg, new Size(
                                 Math.Min((int)(c.X + rSample), 319) - sampleOrg.X,
                                 Math.Min((int)(c.Y + rSample), 239) - sampleOrg.Y));
for (int x = sample.Left; x <= sample.Right; x++)
    for (int y = sample.Top; y <= sample.Bottom; y++)
        if (belong[x, y] != biggest_id)
        {
            int dist = (int)Math.Sqrt(Sqr(x - c.X) + Sqr(y - c.Y));
            if (dist > r1 && dist <= rSample)
            {
                int idx = y < c.Y ? (x < c.X ? 1 : 0)
                                  : (x < c.X ? 2 : 3);
                voters[idx]++;
                sums[idx].X += x;
                sums[idx].Y += y;
            }
        }

PointF adjusted = new PointF();
int vAbove = voters[0] + voters[1],
    vBelow = voters[2] + voters[3],
     vLeft = voters[2] + voters[1],
    vRight = voters[0] + voters[3],
    allVoters = vAbove + vBelow;
if (allVoters == 0)
{
    // abort: no black pixels found
}
else
{
    if (vAbove > 0 && vBelow > 0)
    {
        // split vertically
        PointF above = new PointF((float)(sums[0].X + sums[1].X) / vAbove - c.X, (float)(sums[0].Y + sums[1].Y) / vAbove - c.Y),
               below = new PointF((float)(sums[2].X + sums[3].X) / vBelow - c.X, (float)(sums[2].Y + sums[3].Y) / vBelow - c.Y);
        double dAbove = Math.Sqrt(above.X * above.X + above.Y * above.Y),
               dBelow = Math.Sqrt(below.X * below.X + below.Y * below.Y);
        if (dAbove >= r1 && dAbove <= rSample && dBelow >= r1 && dBelow <= rSample)
            // the split is valid
            adjusted = new PointF((above.X * vAbove - below.X * vBelow) / allVoters, (above.Y * vAbove - below.Y * vBelow) / allVoters);
    }
    if (adjusted.X == 0 && adjusted.Y == 0 &&
        vLeft > 0 && vRight > 0)
    {
        // split horizontally
        PointF toleft = new PointF((float)(sums[2].X + sums[1].X) / vLeft - c.X, (float)(sums[2].Y + sums[1].Y) / vLeft - c.Y),
              toright = new PointF((float)(sums[0].X + sums[3].X) / vRight - c.X, (float)(sums[0].Y + sums[3].Y) / vRight - c.Y);
        double dLeft = Math.Sqrt(toleft.X * toleft.X + toleft.Y * toleft.Y),
              dRight = Math.Sqrt(toright.X * toright.X + toright.Y * toright.Y);
        if (dLeft >= r1 && dLeft <= rSample && dRight >= r1 && dRight <= rSample)
            // the split is valid
            adjusted = new PointF((toleft.X * vLeft - toright.X * vRight) / allVoters, (toleft.Y * vLeft - toright.Y * vRight) / allVoters);
    }
}

Точка adjusted теперь указывает из центра прямоугольника вдоль его поперечной оси:


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

Продемонстрирую работу на ещё одном примере — когда окно попадает в кадр не полностью. Благодаря тому, что мы не пользуемся «концом» гистограммы для распознавания — нас неполное окно нисколько не сбивает с толку.


Написанный мной код был облачён в MRDS-сервис WindowDetector — по образу и подобию стандартного Technologies\Vision\ColorSegment. Мой сервис привязывается к видеокамере, и по каждому обновлению кадра высылает подписчикам UpdateFoundWindow с углом наклона окна в кадре. MRDS-сервис, управляющий роботом, привязывается к WindowDetector точно так же, как привязывается к сканеру баркодов, и совершенно аналогично обрабатывает сообщения от обоих — корректируя в первом случае курс, в другом случае положение.

С детектором окон мой робот ездил по складу довольно резво:


Дальнейшая судьба робота печальна. Всего через час после съёмки этого видео колёсный датчик забился складской пылью, и робот перестал следить за своим положением. При изначальной сборке робота этот датчик ставится самым первым, т.е. для его замены нужно по сути всё разобрать и потом собрать по-новой. Я решил попробовать заменить датчик «на живом» роботе, но по неуклюжести закоротил батарею обо что-то внутри робота, и ездить он перестал совсем. Пришлось его оставить на складской полке рядом с позапрошлогодним роботом — тем, что на основе Roomba. Прямо робокладбище, а не полка.

Так я перед отъездом из Британии и не успел сделать Eddie действующим роботом-грузчиком. Но уже этой весной я, скорее всего, вернусь туда с запчастями, оживлю его, и продолжу испытания.