Месяц назад я писал об определении моим роботом-грузчиком собственного положения. (Жаль, ту статью я запостил в неудачное время в ночь на субботу, так что её мало кто увидел.) Как я отметил, показания колёсных датчиков позволяют роботу определять своё положение достаточно точно — медленно накапливающаяся ошибка корректируется, как только робот сканирует баркод на любой из полок склада. С другой стороны, накапливающуюся ошибку направления корректировать было нечем.
Я обсудил свои затруднения с девушкой-гуманитарием, и спросил, какие ей известны способы ориентации в пространстве. По её словам, в Лондонском музее науки она застала экспозицию, посвящённую ориентации муравьёв по виду вертикально вверх над головой. Посетителям предлагалось взять зеркало и идти по комнате, разглядывая в это зеркало узоры на потолке и ориентируясь лишь по ним. (Карта потолка прилагалась.)
Я решил проверить: что видит на потолке склада мой робот?
Даже на такой низкокачественной записи отлично различимы длинные прямоугольные окна. Все они, естественно, параллельны одной из осей склада, а значит, параллельны и рядам полок. Получается, если «в кадр» попадёт окно и я смогу определить его направление — то я получаю и направление робота, с точностью до 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 — по образу и подобию стандартного TechnologiesVisionColorSegment. Мой сервис привязывается к видеокамере, и по каждому обновлению кадра высылает подписчикам UpdateFoundWindow с углом наклона окна в кадре. MRDS-сервис, управляющий роботом, привязывается к WindowDetector точно так же, как привязывается к сканеру баркодов, и совершенно аналогично обрабатывает сообщения от обоих — корректируя в первом случае курс, в другом случае положение.
С детектором окон мой робот ездил по складу довольно резво:
Дальнейшая судьба робота печальна. Всего через час после съёмки этого видео колёсный датчик забился складской пылью, и робот перестал следить за своим положением. При изначальной сборке робота этот датчик ставится самым первым, т.е. для его замены нужно по сути всё разобрать и потом собрать по-новой. Я решил попробовать заменить датчик «на живом» роботе, но по неуклюжести закоротил батарею обо что-то внутри робота, и ездить он перестал совсем. Пришлось его оставить на складской полке рядом с позапрошлогодним роботом — тем, что на основе Roomba. Прямо робокладбище, а не полка.
Так я перед отъездом из Британии и не успел сделать Eddie действующим роботом-грузчиком. Но уже этой весной я, скорее всего, вернусь туда с запчастями, оживлю его, и продолжу испытания.
Автор: tyomitch
