Поиск порядка объектов с помощью OpenCV 3

Вместе с группой друзей мы пытаемся выполнить задачу компьютерного зрения на Raspberry Pi, кодируя на C ++ с использованием библиотеки OpenCV.

Позвольте мне сначала объяснить задачу.
Существует шаблон, состоящий из 16 отдельных квадратов, каждый из которых имеет красный, желтый или синий цвет. Мы монтируем rasperry pi на квадрокоптер с его модулем камеры и собираем видеопоток шаблона.

Мы должны определить цвета квадратов, что было легко сделать с помощью небольшого исследования в Интернете. Сложность в том, что мы также должны определить порядок квадратов, чтобы сохранить цвета в массиве в порядке.

Пока что мы выполнили фильтрацию желаемых цветов (красный, желтый, синий) для определения квадратов.

пример шаблона, чтобы распознать и наш процесс до сих пор

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

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

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

сильно искаженное изображение

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

То, что я прошу, это предложения алгоритма. Мы полностью идем в неправильном направлении, пытаясь найти углы? Можно ли определить порядок без учета искажений?

Заранее спасибо.