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

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

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

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


Даже на такой низкокачественной записи отлично различимы длинные прямоугольные окна. Все они, естественно, параллельны одной из осей склада, а значит, параллельны и рядам полок. Получается, если «в кадр» попадёт окно и я смогу определить его направление — то я получаю и направление робота, с точностью до 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 действующим роботом-грузчиком. Но уже этой весной я, скорее всего, вернусь туда с запчастями, оживлю его, и продолжу испытания.