nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Аффинный алгоритм, ч.4 - нахождение крена

Часть 1 - нахождение наиболее отдалённой точки
Часть 2 - сортировка против часовой стрелки
Комментарий об операторе упорядочивания
Часть 3 - нахождение матрицы преобразования
Часть 4 - нахождение крена
Математическая интерлюдия
Часть 5 - нахождение масштаба
Финал!

Спустя 3 части и 63 слова кода, мы получили матрицу аффинного преобразования, но пока что интересующие нас значения крена, масштаба (и в результате-дальности) и "ракурсов" все "перемешаны между собой". Её нужно разложить, или факторизовать.

Как оказывается, проще всего начать с вычисления крена. Сам угол мы находить не будем (это не требуется), вместо этого найдём синус и косинус крена, "выкинем" его из матрицы аффинного преобразования, а также составим кватернион крена.

Процедура, выполняющая всё это - одна из наиболее длинных, она занимает 36 слов, и к тому же два раза вызывает процедуру NormSiCo, которая пока более нигде не задействуется, и которая занимает 12 слов, т.е в общей сложности: 48 слов, или 96 байт.

Кроме того, именно в этой процедуре используются наиболее "укуренные" команды ассемблера QuatCore...


Представим нашу матрицу 2х2 в виде произведения матриц крена, масштаба и ракурсов:


Здесь:
T (от слова Transform) - наша матрица аффинного преобразования,
S (Scale) - матрица масштабирования, s-масштаб, причём s>0 (мы не можем подлететь "с обратной стороны" и увидеть изображение зеркально отражённым!)
A (Aspect) - матрица "ракурсов" (когда мы смотрим на плоскость не под прямым углом):



здесь a - величина "ужатия", причём

поскольку мы знаем заранее, что отклонились от нормали к плоскости не более чем на 30°.

ψ - направление, вдоль которого происходит "ужатие".

Наконец, R (Rotation) - матрица поворота на угол ϕ.

Так вот, мы хотим, зная матрицу T, найти кватернион поворота:


и масштаб s. Разбираться с ракурсом мы не хотим - всё равно определим его неоднозначно (что мы отклонились вправо, что влево - а картинка ужмётся одинаково!), и очень криво (попробуй заметить отклонение на 5°, когда оно ужимает картинку по горизонтали на cos(5°)=0,9962).

Для этого мы находим, чему же равна матрица SA (т.е масштаб и ракурс, но без крена):



очень неприятная матрица, но мы можем заметить две вещи:
- она симметричная
- оба диагональных элемента обязаны быть положительными, при наших ограничениях на коэффициенты s и a.

Обозначим как-нибудь её элементы:


Именно её мы должны получить, если умножим нашу матрицу T на матрицу, обратную к матрице поворота:



то есть, мы получаем следующие условия, из которых можем определить матрицу поворота:




Последнее выражение можно переписать, приведя подобные слагаемые:


откуда мы знаем, чему должен равняться tgϕ - это даёт два угла поворота, отличающиеся на 180°. Но с учётом оставшихся двух условий (ненулевые диагональные элементы), один из этих углов мы сразу же отбракуем.

Мы можем записать следующие выражения для синуса и косинуса интересующего нас угла:



где α - нормирующий множитель, обеспечивающий выполнение основного тригонометрического тождества cos2ϕ+sin2ϕ=1:



Найдём сумму двух диагональных элементов:



Если α < 0, то сумма диагональных элементов будет отрицательной, что противоречит нашему требованию, что оба элемента должны быть положительными.

Если α > 0, то сумма диагональных элементов будет положительной, но это ещё не гарантирует, что оба элемента будут положительными. Если это не так, значит, скорее всего мы "зацепились" не за те точки - восприняли какие-то блики как наши мишени. С чистой совестью выдадим признак ошибки - это гораздо лучше, чем на полной скорости долбануться… (возможно, что мы могли бы как ни в чём не бывало построить "кривой" масштаб, зная, что алгоритм сопровождения за него не "зацепится" при всём желании, а данные начинают выдаваться только на сопровождении, но тогда меньше информации для отладки, кто ж его знает, почему не "зацепился", пока оставим так)

Мы приходим к очень простому методу нахождения синуса и косинуса угла крена:

1. Мы находим ненормированные значения:
co=t11+t22,
si=t21-t12.
При дальностях от 10 до 300 метров, на которых будет применяться данный алгоритм захвата, переполнения произойти не может.

2. Нормируем эти значения итеративным способом:

InvNorm=3/2-1/2 (co2+si2 ),
co=InvNorm⋅co,
si=InvNorm⋅si.

этот способ требует только сложений, умножений и сдвигов и легко реализуется в Quat Core.

При максимальной дальности, исходные значения co, si будут наименьшими, так что длина ненормированного вектора (co, si) вместо единицы составит примерно 0,03. Чтобы целиком отнормировать такое значение, нам понадобится 13 итераций.

Приведём код, который находит синус и косинус крена:

    ;состояние регистров:
    ;X = AffineMat,
    ;Y = Points2D = Fx0,
    ;Z = AfTransf,
    ;i=j=k=0,
    ;Inv = 0,
    ;SP указывает на 1 выше адреса возврата
    
    ;из матрицы аффинного преобразовани (2х2) находим синус и косинус угла крена,
    ;а затем и кватернион взаимной ориентации
    FindRoll    proc
    ;нам понадобятся [Z]+[Z+3] и [Z+1]-[Z+2]
                    X           AfTransf
                    Z           QuatY           ;в этом кватернионе построим,используя Y/Z значения как временные
    ;(потом они занулятся)
                    i           1
                    k           3
        @@sico:     Acc         [X+i]
                    Inv         i
                    PM          [X+i^k]
                    [Z+i]       Acc
                    iLOOP       @@sico
    ;введя зело специал. команды [Z+i], [Z+i^3], PMi, можно сэкономить аж 3 строки (по 1 строке на команду!)
    ;пока не будем этого делать...

                    CALL NormSiCo

С помощью лома и какой-то матери мы всё-таки превратили строки
co=t11+t22,
si=t21-t12

в цикл! А именно, на первой итерации, когда i=1, мы заносим в аккумулятор значение [X+1], т.е Tyx. Затем присваиваем Inv=1, и вычитаем (поскольку Inv=1) [X + 1^3] = [X+2], т.е Txy. Напомним, что ^ означает оператор XOR - побитовое исключающее ИЛИ. Такая странная адресация очень полезна для перемножения кватернионов, нахождения векторного произведения, умножения комплексных и дуальных чисел, см здесь. И здесь тоже сослужило некоторую службу :)

На второй итерации, мы заносим значение [X+0], т.е Txx, затем присваиваем Inv=0, и прибавляем [X + 0^3]=[X+3], т.е Tyy.

Значения мы заносим в Y- и Z- компоненту кватерниона крена, в качестве временного хранилища (использовать [SP], [SP+1] нельзя, потому что вот-вот надо процедуру вызывать, которая их затрёт, хотя можно было бы [SP+1] и [SP+2], но тогда адресация неудобная, или волевым усилием инкрементировать SP перед вызовом процедуры, чтобы локальные переменные не трогал, а потом назад вернуть, в общем ну нафиг!).

Затем мы вызываем процедуру нормировки, вот её код:

;по адресу Z лежит два числа, двухмерный вектор
;нужно его отнормировать.
;регистр j должен при вызове быть нулевым!
NormSiCo    proc
                i           12 ;если "передавать" i как аргумент, то можно регулировать число итераций
;при первом вызове нужно 13 итераций, при втором хватило бы и 6 итераций,
;но мы пока пытаемся оптимизировать по памяти кода, а не по быстродействию
;по быстродействию запас в 300 раз пока что...
@@norm:         k           1       ;две компоненты
                ZAcc        0       ;выставить 3/2
@@innorm:       SQRSD2      [Z+2j+k]    ;вычесть половину от квадрата 
                kLOOP       @@innorm
                ;теперь на содержимое аккумулятора надо домножить Si и Co
                k           1
                C           UAC
@@inmult:       MULSU       [Z+2j+k]    ;уможение знакового B на беззнак. C
                [Z+2j+k]    Acc     ;вернули на место!
                kLOOP       @@inmult                
                iLOOP       @@norm
                JMP         [--SP]
NormSiCo    endp


Мы делаем 13 итераций нормировки. Каждый раз начинаем с нахождения величины, обратной к норме. Мы видим, как специальной командой ZAcc (мы упоминали её в предыдущей части) заносим в аккумулятор значение 3/2.

Затем используется команда SQRSD2 (Square - subract - divided by 2), по сути: Acc = Acc - Src*Src / 2. Такая команда вполне вписывается в возможности нашего АЛУ - он заносит одно и то же значение в B и C, потом сдвигает B вправо - и начинает "умножение с накоплением".

Дальше мы видим интересный адрес UAC, что сразу наводит на мысли о United Aerospace Corporation, но это всего лишь сокращение от Unsigned Acc. Наш аккумулятор может содержать значения от -2 до 2-2-30. Когда мы используем адрес Acc, на шину коммутируется "насыщенный" результат, т.е слишком большие значения "зашкаливают" на 32767, а слишком маленькие - на -32768. Здесь же мы просим выдать результат "как есть", отлично понимая, что скорее всего результат будет больше единицы, в диапазоне 1..1,5, т.е наши синус и косинус очень малы. Если воспринимать 16-битное значение как знаковое, это будут отрицательные значения. Но если как 16-битное без знака - то это будет ровно то, что мы хотим!

Следующая "новая" команда - MULSU (Multiply Signed to Unsigned), позволяющая умножить наши синусы-косинусы на число в диапазоне 0..2-2-15.

Наличие беззнаковых чисел в нашей "системе команд" спасает формат чисел 1.15 (1 бит перед запятой, 15 бит после запятой) - не будь их, и любое умножение производилось бы на числа -1..1-2-15, без возможности по-простому УВЕЛИЧИТЬ число!

Данная процедура далеко не идеальна: увеличение изначально маленьких чисел "небольшими порциями", по 1,5, приводит к округлениям на каждом шагу и накоплению ошибок. К примеру, если мы начали с (500;-1), то "минус единица" при каждом умножении на 1,5 так и будет оставаться минус единицей. В итоге, мы получим ответ (32767;-1), что приводит к ошибке по углу в atan(1/500) - atan(1/32767) = 0,11° - не есть хорошо! Если бы мы начали с масштабирования с помощью сдвигов влево, мы бы получили:
(500;-1),
(1000;-2),
(2000;-4),
(4000;-8),
(8000;-16),
(16000;-32),
(32000;-64),
а затем уже добьём по методу Ньютона до (32767; -66), что даёт угловую ошибку в 0,0008°. Ясное дело, что при округлении значения синуса до -1 мы уже наверняка ошиблись максимум на 0,05°, но хотя бы не внесли новой ошибки.

Но опять: такой "совместный" сдвиг выполняется уж больно коряво - нужно таскать взад-вперёд несколько значений, проверяя каждое из них на "переполнение". Пока не хочу. Всё та же мотивация: даже ошибка в угле на 0,11° может привести к уходу точки не более чем на 0,03 пикселя, т.к масштаб маленький и расстояние между ними невелико.

Теперь надо найти кватернион крена. Мы знаем его вид:


Мы знаем синус и косинус угла и крена, а нужны синус и косинус ПОЛОВИННОГО угла! Мы рассматривали эту задачу в части 6 3/4 ликбеза по кватернионам, но тогда мы ещё не взялись вплотную за ассемблерный код, поэтому не довели задачу до конца.

А именно, рассмотрели случай "ненормированного" вектора направления, из-за чего пришлось находить корень из суммы квадратов, а потом всё равно нормировать. Нет уж, нормировка - одна из самых простых операций, как мы увидели (всего 12 слов), и она всё равно нам была необходима, чтобы "довернуть" матрицу аффинного преобразования в обратную сторону, "выкинув" из неё крен. Поэтому, учитывая единичную норму наших "синуса и косинуса", упростим выражения для компонентов кватерниона.

Если co > 0, то







в противном случае,






Норма кватерниона, найденного таким способом, будет находиться в диапазоне от до 1. Как показал опыт, для его точной (до последнего бита) нормировки достаточно 6 итераций. Но из-за своей жадности, мы вызовем всё ту же процедуру NormSiCo (два нулевых значения мы добавим позже), которая сделает 13 итераций. Если будет казаться, что нам не хватает производительности, то так и быть, будем инициализировать регистр i перед вызовом процедуры, задавая таким образом необходимое число итераций :)

Но сначала надо получить компоненты кватерниона из синуса и косинуса крена. Формулы, приведённые выше, достаточно простые, но всё-таки нам нужно полноценное "ветвление", if-then-else, а так не хочется - там же один прыжок, чтобы попасть в блок 'else', а второй, чтобы из блока 'then' перескочить к дальнейшему коду, пропустив 'else'.

Из жадности, мы решили ввести весьма укуренные команды, благодаря которым этот код займёт всего 5 слов:

ABSP1D2 [Z+2j+k]    ;взять модуль B, прибавить единицу, и поделить на 2
;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
Y       QuatA
[Y+S]   Acc
DIV2    [Z+2j+1]
[Y+~S]  Acc


Первая команда расшифровывается как "Abs plus 1 div 2", она находит выражение


которое нам нужно посчитать на обеих ветвях. Собственно, наш АЛУ не особенно любит деление, корни и тригонометрию (точнее, совсем не любит), а вот найти модуль числа, поделить на два, прибавить константу, да хоть даже найти максимум двух чисел - это всегда пожалуйста! Подозреваю, что с verilog-реализацией АЛУ придётся попотеть в своё время (которое уже не за горами), но ничего особо зверского тут нет.

Но затем в зависимости от знака co, надо поместить результат выражения либо в λ0, либо в λx. В первую очередь, заносим в регистр Y адрес λ0, а затем применяем вторую "укуренную" инструкцию [Y+S]. Здесь S - это флаг знака, 0, если результат положительный и 1, если результат отрицательный. Правда, введя эту инструкцию, мы всё-таки вынуждаем себя поставить отдельный регистр для флага S, и обновлять его только в некоторых арифметических операциях. Скажем, во всех, кроме наших ABSP1D2 и DIV2. Если так сделать, то у нас сохранится знак с процедуры нормировки si,co, причём последнее совершённое арифметическое действие - это отнормировать co, поэтому с тех пор у нас держится правильный знак!

Следом идёт команда DIV2, чтобы поделить на два si, это вторая компонента. По сути, это просто сдвиг вправо на единицу. И она не должна менять флаг S (sign).

И последняя "укуренная команда": [Y+~S] - тут мы прибавляем "инверсию" знака, т.е 0 при отрицательном результате и 1 при положительном.

И теперь надо отнормировать кватернион (его первые 2 компоненты):
Z       QuatA
CALL    NormSiCo    ;подготовили первые 2 компонента кватерниона


Ура, кватернион крена готов! Его будет использовать алгоритм сопровождения.


Осталось "выкинуть" крен из матрицы афинного преобразования, помножив её на обратную матрицу поворота!
Вот код, осуществляющий данное умножение:
                    X           QuatY
                    Y           AfTransf
                    i           1   ;номер строки результата, и строки co/si
        @@i_loop:   j           1   ;номер столбца результата, и столбца AfTransf
        @@j_loop:   k           1   ;номер столбца co/si и строки AfTransf
                    ZAcc        2   ;обнулить до 1/2 мл. разр
        @@k_loop:   C           [X+i^k] 
                    FMPM        [Y+2k+j]
                    kLOOP       @@k_loop
                    [SP+2i+j]   Acc
                    jLOOP       @@j_loop
                    iLOOP       @@i_loop

И снова наши "укуренные" команды в деле: [X+i^k] совместно с FMPM (Fused Multiply-Plus-Minus) позволяют не строить в явном виде матрицу поворота


поскольку XOR номера строки и номера столбца правильно указывает нам, где поставить cos, а где sin, а таблица, которая выбирает знак через регистры i,k, Inv, правильно расставляет знаки. Подробнее см. здесь.

Мы видим, что результат сохраняется на стек, потому что сделать умножение "на месте", увы, не получится - как только мы посчитаем одно значение из 4 и поставим его на своё место, как все остальные вычисления станут ошибочными. Так что теперь надо перенести значения из стека назад:

                    Z           AfTransf
                    i           3
        @@out_loop: [Z+i]       [SP+i]  ;помещаем ответ куда надо
                    iLOOP       @@out_loop                  


и наконец, поставим нули в компоненты Y,Z кватерниона крена:
                    k           1
        @@clean:    [X+2i+k]    0
                    kLOOP       @@clean

(как обычно, после предыдущего цикла у нас уже нулевой i, так что можно поставить более сложную адресацию, сэкономив количество различных "инструкций" ).

Вот, собственно, и всё...

Давайте посмотрим, как это работает. Сначала возьмём тот же пример, что в предыдущих частях, там мы получили матрицу


Сначала поставим точку останова (breakpoint) перед вызовом процедуры нормировки:


В переменной QuatY хранится "косинус", 872, т.е сумма диагональных элементов, а в QuatZ - "синус", -1, т.е разность двух оставшихся. Пока всё правильно.

Теперь жмём Step Over (сделать один шаг, но не "погружаться" внутрь процедур):


Видим, что процедура нормировки получила "вектор" (32767; -1) - это тот самый "наихудший случай", когда угол уползает сильнее всего, потому как нулевая компонента остаётся нулевой - и это правильно, а вот -1 при умножении на 1,5 так и остаётся -1, 13 раз подряд! В итоге имеем ошибку по углу atan(1/872) - atan(1/32767) = 0,064°. Ну и пускай :)

Также можно заметить результат работы "насыщаемой арифметики" - в аккумуляторе лежит значение 32768, а в память пришло 32767, потому как это знаковые числа, 32768 "превратилось бы" в -32768, и дальше всё пошло бы под откос.

Умножение матрицы на 32767 вместо 32768 приводит к ошибке по масштабу менее 0,003%, при допустимой ошибке 1,67% (на 5 метров при дальностях 100..300 метров) - это мы как-нибудь попытаемся пережить :)

Далее смотрим, какой получился ненормированный кватернион:


Внезапно - это кватернион нулевого поворота. Ну да, когда (-1) поделили на два, пришлось округлить уже до нуля. Ошибка вычислений по углу возросла до 0,066°, но как ни странно, в итоге ответ вышел совершенно верным - при генерации изображения, по которому мы нашли точки, мы задали именно нулевой крен! И напомним: это в точности НАИХУДШИЙ СЛУЧАЙ - максимальная дальность (поэтому очень маленькая картинка), и САМАЯ неудобная компонента -1.

Тут даже округление банкира не поможет: мы умножаем не в точности на 1,5, а на 1,5 - 1/2 ((1/32768)2+(872/32768)^2) = 1,499646, что в 32-битном аккумуляторе явно отличается от 1,5, поэтому единица, помноженная на такое число, так и будет оставаться единицей!

Но это всё ловля блох - нам с 300 метров вообще не требуется крен определять, только для входа в сопровождение, а для этого хватило бы и плюс-минус 10°.

Осталось посмотреть, как "выкидывается" крен из матрицы аффинного преобразования, для чего поставим Breakpoint в начале следующей процедуры. (мне лениво было рисовать красный кружок взамен checkbox'а, так что галочки слева от кода - это оно и есть)


Да никак не выкидывается! И это тоже сколько-нибудь понятно: значения 0 и 1 вне главной диагонали надо условно разделить на "синфазную компоненту" (1/2; 1/2), которая отвечает за ракурс, и "противофазную" (-1/2; 1/2), отвечающую за крен. Вот она должна была быть выкинута, но не хватило точности.

Никакой трагедии в этом нет, хотя несколько "неаккуратно", что на этом этапе должна была получиться симметричная матрица, но не получилась. Можно было ещё и здесь пожадничать - посчитать 3 компонента из 4, но у меня есть ощущение, что оставив два этих значения, мы чуть-чуть сохраним точность, как станет ясно в следующей части.

А теперь рассмотрим другие исходные координаты, соответствующие крену 90 градусов и дистанции 30 метров:

Fx0		Int16	2996
Fy0		Int16	4794
Fx1		Int16	12794
Fy1		Int16	3974
Fx2		Int16	2771
Fy2		Int16	448
Fx3		Int16	5756
Fy3		Int16	-5508


Это соответствует такой картинке:


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

По окончании двух первых процедур (FindMDD3 и SortCCW), точки немножко "перемешиваются":


то есть, нумерация теперь такая:


То, что доктор прописал - начинаем с самой отдалённой, и против часовой стрелки!

Далее, вычисляем матрицу аффинного преобразования:


То есть:


Находим "ненормированные" синус и косинус:


Нормируем их:


Из (4; 8817) получили (16; 32767), получив ошибку по углу |atan(4/8817)-atan(16/32767)| = 0,002° - совсем другое дело :)

Заметим, что здесь синус практически единица, а косинус - ноль, то есть поворот на 90 градусов мы "отловили" чётко!

Построим ненормированный кватернион:


Отнормируем его:


Если поделить эти значения на 32768, имеем:


значения подозрительно напоминают единицу деленую на корень из двух :) И действительно, отличие от угла в 45 градусов (в кватернионе же участвует половинный угол!) составляет всего 0,015°.

Наконец, посмотрим, как из матрицы аффинного преобразования "выкинули" крен:


В "нормальной записи":


В этот раз матрица получилась симметричной - всё как в аптеке.

Следующая стадия: из получившейся матрицы "вычленить" масштаб, постаравшись проигнорировать "ракурсы".


Продолжение следует...
Tags: tex, ПЛИС, кватернионы-это просто (том 1), математика, программки, работа, странные девайсы
Subscribe

  • Лестница для самых жадных

    В эти выходные побывал на даче, после 3-недельной "самоизоляции". Забавно, как будто зима началась! Особенно грязные галоши остались на улице, в…

  • Возвращаемся к макету

    Очень давно макетом видеоизмерителя параметров сближения не занимался: сначала "громко думал" по поводу измерения его положения на аппарате, а потом…

  • Минутка живописи

    В процессе разгребания содержимого квартиры (после нескольких ремонтов) дошёл, наконец, и до картин. В кои-то веки их повесил. Куда их вешать -…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments