nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Аффинный алгоритм ч.3 - нахождение матрицы преобразования

Часть 1 - нахождение наиболее отдалённой точки
Часть 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 пикселя за счёт ошибок вычислений.

Итак, преобразование мы нашли, "по методу наименьших квадратов", как это ни странно! (просто вся "толстая часть" была посчитана заранее) Осталось за малым: разложить (факторизовать) матрицу преобразования на масштаб и крен, попытавшись проигнорировать "ракурсы".


Продолжение следует...

Poll #2095620 Ассемблер

Какой ассемблер вам больше нравится?

x86
3(33.3%)
RISC (например, AVR)
1(11.1%)
QuatCore
0(0.0%)
Какой-то ещё (напишите в комментариях)
1(11.1%)
Никакой не нравится!
4(44.4%)
Tags: ПЛИС, кватернионы-это просто (том 1), математика, программки, работа, странные девайсы
Subscribe

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 4 comments