Часть 2 - сортировка против часовой стрелки
Комментарий об операторе упорядочивания
Часть 3 - нахождение матрицы преобразования
Часть 4 - нахождение крена
Математическая интерлюдия
Часть 5 - нахождение масштаба
Финал!
Итак, мы полностью идентифицировали 4 точки, которые увидели на фотоприёмной матрице:
Кроме того, в нашу память заранее "зашита" реальная конфигурация этих катафотов на объекте (данные цифры весьма ориентировочные и используются просто для моделирования):
A0 (-0,29; 0,678; -1,928), A1 (-0,29; 0,834; -0,05), A2 (0; 0; 0) A3 (-0,29; -1,14; -0,579),
Первая координата - это "глубина" - показано, что 2-я точка (центральная) выдвинута на 29 сантиметров "на нас". В аффинном приближении мы проигнорируем это, считая, что все 4 точки на самом деле лежат в одной плоскости. (До сих пор открытый вопрос - какую плоскость лучше всего взять? Кажется, что это должно быть "среднее арифметическое", но моделирование уверяет: меньше всего систематической ошибки выходит, если взять плоскость, на которой лежат 3 точки из 4, т.е X=-0,29.)
Все координаты здесь указаны в метрах.
Мы ставим задачу: найти такое преобразование
которое давало бы наилучшее совпадение с точками, которые мы "увидели" на фотоприёмной матрице.
Если за наилучшее совпадение принять минимум суммы квадратов разностей (скука! но работает же...), то выведем такой результат:
К счастью, нам вовсе не обязательно считать всю эту штуку "на борту". Первые две матрицы содержат заранее известные величины - "реальные" (в метрах) координаты катафотов на стыковочном узле. Поэтому можно вычислить их "на Земле", помножить друг на друга и получить в результате матрицу 3х4, которую уже "на борту" мы умножим на матрицу 4х2 (координаты 4 точек фотоприёмной матрицы, обязательно В ПРАВИЛЬНОМ ПОРЯДКЕ, ради чего и затевались первые 2 части) и получаем матрицу 3х2 коэффициентов аффинного преобразования.
Для наших точек должно получиться нечто такое:
Данная матрица обладает интересным свойством: сумма элементов на первой строке, а также на второй строке равна нулю, а на третьей строке - единице. Это, разумеется, не случайность: если изображение на фотоприёмной матрице сдвинется в любом направлении (например, ко всем координатам X прибавится единица, а к координатам Y - двойка), то получившиеся прибавки должны целиком уйти в вектор параллельного переноса (два нижних элемента), тогда как матрица T (2x2) должна остаться неизменной. Так оно и выходит.
Кроме того, нижняя строка в данный момент является "безразмерной" (наши числа Fx0, Fx1 и т.д. выражены в пикселях, и параллельный перенос мы меряем в пикселях), тогда как числа в двух верхних строках имеют размерность 1/м, чтобы коэффициенты матрицы получили размерность пикселей на метр.
Но такая матрица нам не очень удобна. Помножив её на некоторое "волшебное число" 0,8473, мы получим возможность практически напрямую пересчитывать масштаб в дальность в метрах. Это число берётся из величины 2417 пикселей на радиан - т.е углового разрешения объектива и матрицы. Мы берём от неё обратную величину, и умножаем на степени двойки, пока не придём к максимальному значению, меньше единицы. Это и будет 0,8473 :)
И ещё мы поделим нижнюю строку на 2 - так надо, чтобы в процессе вычисления вектора параллельного переноса (уже трёхмерного) у нас не могло получиться переполнения. Может, правильнее даже поделить её на 4 - если помните, в первой части у нас оказался разный масштаб вектора по оси X (дальность) и осям Y,Z (поперечные направления), с надеждой чуть повысить точность работы. Но возможно, что последующий алгоритм (сопровождения) окажется сложнее чем надо, тогда приведём всё к одному масштабу.
Наконец, представляем эту матрицу в целых числах, по сути помножив все значения на 32768 и округлив до ближайшего целого. Получается как-то так:
(слова "как-то так" обозначают, что проделав все указанные операции с матрицей, указанной выше, вы не получите эту, поскольку она сама была показана с точностью в 4 дес. разряда, и разрешение матрицы 2417 - это примерное значение, тогда как эти числа высчитаны без промежуточных округлений)
Мы представляем её в памяти с помощью таких строк:
.rodata ;константы, которые держатся весь сеанс (если только по МКО не поступил массив полётного задания, который их переопределяет!) ;заранее посчитанная матрица для нахождения коэф. аффинного преобразования ;МДД3-МДД1-МБД-МДД2 AffineMat: AfMat00 Int16 4090 ;адр -18 AfMat01 Int16 10092 AfMat02 Int16 310 AfMat03 Int16 -14492 AfMat10 Int16 -13992 ;адр -14 AfMat11 Int16 8627 AfMat12 Int16 7376 AfMat13 Int16 -2011 AfMat20 Int16 -1199 ;адр -10 AfMat21 Int16 5752 AfMat22 Int16 5807 AfMat23 Int16 3495
видим новую директиву .rodata (Read-only data), которая не оказывает никакого влияния на содержание памяти ПЛИС (у меня там нет никаких страниц), зато на неё реагирует эмулятор, поставив иконки "только для чтения" на эти ячейки, а также грязно ругаясь, если программа попытается в них что-нибудь записать. Собственно, и комментарий показывает, что мы должны иметь возможность изменять эти значения, но не в этих математических процедурах, а в других, занимающихся информационным обменом.
Ну а дальше всё просто - записываем слева направо, сверху вниз.
Директива Int16 работает точно так же, как dw, но опять же "пинает" эмулятор, чтобы он изобразил эти числа как знаковые. В остальном разницы нет.
Рассмотрим, наконец, код процедуры, которая умножает эту матрицу на матрицу 4х2, составленную из наших 4 точек:
;состояние регистров:
;X = Fx1,
;Y = Points2D = Fx0,
;Z = Fx2
;i=j=k=0,
;Inv=0,
;по уже идентифицированным (переставленным в нужном порядке) точкам
;сначала находит коэф. аффинного преобразования,
Compute4PointAffine proc
X AffineMat
;Y, как и надо, УЖЕ указывает на Points2D
Z AfTransf
;по сути, Z = X * Y, где все операнды - матрицы.
j 2 ;номер строки результата (и строки AffineMat)
@@j_loop: i 1 ;номер столбца результата (и столбца Points2D)
@@i_loop: k 3 ;номер столбца AffineMat и строки Points2D
ZAcc 2 ;обнулить до 1/2 мл. разр
@@k_loop: C [X+4j+k]
FMA [Y+2k+i]
kLOOP @@k_loop
;очередной результат готов...
[Z+2j+i] Acc
iLOOP @@i_loop
jLOOP @@j_loop
;вот, какбе и всё...
Compute4PointAffine endp
Процедура занимает всего 12 слов, или 24 байта. Так получается благодаря "правильным" (которые мы сделали ровно "для себя") режимам адресации с одним базовым и двумя индексными регистрами, а также команде FMA (Fused Multiply-Add - умножение с накоплением), которая не только укорачивает код, но и обеспечивает высокую точность работы: промежуточные результаты хранятся в 32-битном аккумуляторе.
Также мы наблюдаем здесь интересную "команду" (точнее, адрес Dest) ZAcc. С её помощью мы можем поместить в аккумулятор 4 значения, которые другим способом поместить было бы сложно:
ZAcc 0 : Acc = 0x60004000 ("3/2"),
ZAcc 1 : Acc = 0xA0004000 ("-3/2"),
ZAcc 2 : Acc = 0x00004000 ("0"),
ZAcc 3 : Acc = 0x80004000 ("2", хотя на самом деле "-2", но мы наплюём на переполнение и получится как надо)
3/2 и -3/2 имеют сакральный смысл при нормировке векторов и кватернионов, двойка - для нахождения обратной величины по методу Ньютона, а здесь нам нужно самое частое значение, "как бы ноль". Вместо нуля мы кладём 1/2 от "младшего разряда", точнее, того разряда, который становится младшим, когда мы забираем из аккумулятора 16-битное значение. Таким образом, простое отсечение младших разрядов "превращается" в округление до ближайшего целого, что вдвое повышает точность операций.
Ещё одно замечание: если мы возьмём сумму модулей всех коэффициентов матрицы 3x4 по любой из строк, такая сумма не превысит 32768, что является гарантией, что какие бы входные данные мы не послали, а переполнения произойти попросту не может!
Наконец, посмотрим в эмуляторе результат выполнения этой процедуры:
Верхние ячейки памяти имеют ту самую иконку "только для чтения". Дальше, отмеченные звёздочками, мы видим наши правильно отсортированные точки с фотоприёмной матрицы, а чуть ниже - коэффициенты аффинного преобразования. По сути, мы получили
Нижняя строка - это транспонированный вектор параллельного переноса, мы видим сколько-нибудь существенный сдвиг "вправо" (по оси X, хотя в трёхмерных частях нашей задачи он становится Y, та ещё каша...), и почти отсутствующий сдвиг "вниз".
Верхние две строки - это матрица 2х2 линейного преобразования. Она почти что диагональная, что означает - крена нет. Очень близкие значения по диагонали говорят - ракурса нет. Есть только масштаб, что соответствует модели - такую картинку мы видим, если стоим "строго напротив", на расстоянии 300 метров.
Увы, уже здесь можно заметить, что мы задействуем лишь младшие 9..10 бит наших 16-битных значений - так получается, потому что масштаб сейчас мал, а мы "оставили запас" на подлёт на расстояние практически в 10 метров, когда изображение еле-еле будет умещаться на экране. В итоге, мы всё-таки умудряемся несколько потерять точность за счёт "преждевременных округлений".
Можно было бы реализовать более сложный метод, когда мы пытаемся "отнормировать" эту матрицу ещё до того, как отсекли 32-битные значения до 16 бит, это могло бы чуть поднять точность, но сложнее в реализации. И пока моё мнение таково: это у нас "алгоритм захвата", его точности должно хватать для того, чтобы устойчиво войти в сопровождение (там алгоритм СОВСЕМ другой). Если мы устойчиво входим в сопровождение, значит, со своей ролью этот алгоритм справляется. А по сути, для входа в сопровождение мы должны "предсказывать" координаты точек на фотоприёмной матрице с точностью в "ширину точки", т.е на больших дальностях - 2..3 пикселя. Т.е наша "метрика" в данном случае - пиксели и есть. Начиная работать с ценой младшего разряда в 1/64 пикселя и сохраняя, по сути, тот же масштаб, мы заведомо не внесём ошибку сильно больше 1/16 пикселя за счёт ошибок вычислений.
Итак, преобразование мы нашли, "по методу наименьших квадратов", как это ни странно! (просто вся "толстая часть" была посчитана заранее) Осталось за малым: разложить (факторизовать) матрицу преобразования на масштаб и крен, попытавшись проигнорировать "ракурсы".
Продолжение следует...
Какой ассемблер вам больше нравится?
x86
3(33.3%)
RISC (например, AVR)
1(11.1%)
QuatCore
0(0.0%)
Какой-то ещё (напишите в комментариях)
1(11.1%)
Никакой не нравится!
4(44.4%)
