nabbla (nabbla1) wrote,
nabbla
nabbla1

Category:

Аффинный алгоритм - финал!

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

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

(как известно, фотографы всегда ищут приключений на свою заднюю главную точку)

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

С кватернионом мы худо-бедно разобрались в части 2, а именно, определили крен, который в этой задаче имеет право быть произвольным, и его устранение совершенно критично для последующего алгоритма сопровождения. А вот "пассивные углы", они же "ракурсы", мы так и не стали определять - аффинный алгоритм с ними "не дружит".

Теперь, спустя 134 слова кода (268 байт), осталось только построить радиус-вектор.

Процедура, делающая это, имеет романтическое название FindVector и занимает 24 слова (48 байт).


Но сначала поясним, как это вообще делается "математически".

Во-первых, про загадочную "заднюю главную точку" объектива. Это всего лишь "очень умный" способ сказать "центр линзы". Когда мы упрощённо рисуем работу объективов, то рисуем одну-единственную линзу:


(опять стащил с яндекс.картинок)

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

Но когда линз больше, и они все "толстые", свет может проходить через них куда более причудливым образом, и чтобы абстрагироваться от всего этого, и вводят понятие задней главной точки. Это "виртуальная" точка объектива, через которую от каждой точки объекта должен пройти "прямой луч" до соответствующей точки изображения (пусть даже в реальности он идёт не так). В итоге, мы снова рисуем в этом месте "тонкую линзу" и забываем о реальной оптике, как о страшном сне.

Если яркая точка расположена на расстоянии X по оптической оси от центра линзы, и смещена вверх на расстояние Y, то координата этой точки на фотоприёмной матрице равна:



Здесь α - "разрешающая способность" оптико-электронной системы, для нашего объектива и нашей матрицы, α≈2417 пикселей/радиан. Эта величина вычисляется по фокусному расстоянию объектива и физическому размеру пикселя, но чтобы окончательно выпендриться, можно включить сюда ещё и гиперфокальное расстояние, что уточнит ответ на несколько процентов (объектив настроен не на "бесконечность", а чуть ближе, поэтому фотоприёмная матрица отодвинута от фокальной плоскости, что чуть меняет масштаб изображения).

Величина


- не что иное, как наш масштаб, выраженный в пикселях на метры. Т.е, если точка перемещается поперёк оптической оси на 1 метр, то изображение этой точки переместится на α/X пикселей.

Так что, определив масштаб (см. часть 5), мы можем определить дальность, т.е X-компоненту нашего вектора:



s (scale) - найденный нами масштаб.

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

Давайте проверим: при расстоянии 300 метров мы получали масштаб 874,336 при взгляде "ровно перед собой", и 877,376 при взгляде под углом 30°.

Возьмём обратную величину:
1/874,336 = 0,00114372506679354

Не похоже ни на что полезное. Но помножим её на 218, и получится:
218 / 874,336 = 299,82

Во втором случае,

218 / 877,376 = 298,78

Это и есть ответ в метрах!

И остаётся лишь узнать координаты Y,Z нашего радиус-вектора. Мы уже определили параллельный перенос в пикселях, или, что то же самое - в радианах, и теперь достаточно только домножить их на уже найденный X - и мы получим полный вектор. Если помните, нижняя строчка матрицы 3х4 также была домножена на "мантиссу" α, так что и там никаких дополнительных "размерных коэффициентов" нам не понадобится.

Теперь к реализации на QuatCore (наверное, пора переименовать его в ℍ-core, а то поисковики вечно мне исправляют на Quad).

Как ни странно, для представления радиус-вектора мы собираемся использовать "самопальные" числа с плавающей точкой! А именно, радиус-вектор состоит из "общей" экспоненты Exp, одного беззнакового 16-битного числа Tx (translation X) и двух знаковых 16-битных чисел Ty, Tz.

Экспонента в нашей задаче будет принимать значения от 0 до 9, чтобы представить диапазон дальностей от 0,5 метра до 300 метров.

Смещение по оси X всегда должно принимать значения от 32768 до 65535, т.е самый старший бит должен быть единичным. Наконец, Y,Z могут принимать любые значения от -32768 до 32767.

Посмотрим код для нахождения Exp:

    ;состояние регистров:
    ;X = MetricW = ManhW,
    ;Y = Z = AfTransf,
    ;i=j=0,
    ;k=3,
    ;Inv=1,
    ;SP указывает на 1 выше адреса возврата
    ;в акк. лежит удвоенный масштаб
    ;возможно, уже превышая установленные для него рамки
    
    ;наконец, строим 3-мерн вектор параллельного переноса
    FindVector  proc
    ;сейчас ещё одна команда, которую предстоит сделать
    ;сдвиг влево непосредственно внутри акк., до тех пор, пока не вызовет переполнения!
    ;его мы определяем по отр. знаку.
    ;и подсчитываем кол-во итераций
    ;для этого в кои-то веки нужен положительный инкремент :)
    ;тут нужен цикл с предусловием
                    JO          @@endLoop
        @@loop:     SHLAcc      k++         ;вообще, аргумент не требуется, k++ "сам по себе"
                    JNO         @@loop      
    ;ага, сделали
        @@endLoop:  [SP+1]      Acc         ;финт ушами: заносим значение 32767, ибо нефиг! (другими способами его сложно раздобыть)
                    UDIV2       UAC ;"снимаем" переполнение, вернув диапазон 16384..32767. Возможно, ставим правильное округл. заодно
    ;значение k надо бы сунуть в память, в Exp, но дайте глянем, как дальше дела пойдут...
    ;ведь мы можем использовать знач. 0..127 в поле Dest как непоср. адрес в памяти, чего добру пропадать!
                    X           Exp
                    [X]         k       ;а пока так...



К началу выполнения процедуры, у нас в аккумуляторе лежит удвоенный масштаб. На дальности в 300 метров он довольно маленький (874), но по мере приближения он начинает расти, на расстоянии 10 метров он практически достигнет 32768, а чуть ближе может и превысить их. Этот алгоритм на расстоянии менее 8 метров становится абсолютно бесполезен, поскольку большинство катафотов уже выйдут за поле зрения нашего объектива, так что всё идёт как задумано :)

Мы хотим найти обратную величину, но это страшно неудобно при работе с целыми числами, так что мы хотим загнать этот масштаб в диапазон 16384..32767, а уже затем обратить по методу Ньютона (ответ будет в диапазоне 32768..65535). А пока мы будем сдвигать его, посчитаем итерации - это и будет наша экспонента!

В этот раз у нас выполняется цикл "с предусловием" (наподобие циклов while): пока мы не довели аккумулятор до переполнения, продолжаем сдвигать его влево. Причём аккумулятор может быть переполнен с самого начала, если дистанция составляла 8..10 метров, поэтому приходится сделать дополнительную проверку.

JO означает Jump if Overflow,
JNO - Jump if Not Overflow,
SHLAcc - Shift Left Accumulator. Данная "команда" (точнее, адрес Dest) полностью игнорирует содержание шины данных, она всегда сдвигает аккумулятор на один бит влево. А "в правой части", тем временем, мы прибавляем единичку к регистру k, чтобы "время зря не терять". Очень удачно получилось, что с прошлых вычислений k=3, именно такое начальное значение и было нам нужно!

Следующая строка совершенно упоротая:
        @@endLoop:  [SP+1]      Acc


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

Ничего подобного: мы заносим в [SP+1] число 32767. Ведь мы попадаем на эту строку только после наступления переполнения, причём, поскольку мы сдвигали положительное число, то и переполнение у нас "в плюс", а значит, адрес Acc даст нам "насыщенный" до 32767 результат, за иным мы должны были бы взять UAC (Unsigned ACc). Это число - хорошее "нулевое приближение" для метода Ньютона.

Всем хороша наша архитектура TTA, но инициализировать память каким-то конкретным 16-битным значением бывает очень тяжело... Так что если у нас подвернулась такая оказия - почему бы не воспользоваться :)

Далее, мы делаем сдвиг в обратную сторону, чтобы выйти на диапазон 16384..32767.

А затем снова сталкиваемся с трудностями адресации - чтобы просто занести значение регистра k в переменную Exp, уходит 2 слова... Как видно в комментариях к коду, мне очень захотелось всё-таки ввести "прямую адресацию" хотя бы на запись в память: если адреса 0..127 в Src заняты "непосредственными значениями", то те же самые 0..127 в Dest пока свободны, потому как выражение

1 = 50 - бессмыссленно, мы не можем изменить значение единицы :) (хотя поговаривают, что где-то в java такое можно случайно провернуть, поскольку там всё есть объект, в том числе маленькие литералы :))

Поэтому оно могло бы означать

[1] = 50, т.е положить значение 50 по адресу 1. Но пока я колеблюсь...

Теперь пора найти величину, обратную к масштабу, по методу Ньютона.

Вот соответствующий код:

                    [SP]        UAC
                    k           3
        @@Newton:   ZAcc        3       ;занести двойку
                    C           [SP+1]  ;наше приближение к обр. величине
                    UFMS        [SP]    ;Acc = 2 - a * x
                    MULU        UAC     ;Acc = Acc * x = x * (2- a*x), т.е итерация метода Ньютона
                    [SP+1]      UAC
                    kLOOP       @@Newton            
                    X           Tx
                    [X]         UAC


Итак, у нас задействовано две локальные переменные. В [SP] лежит значение масштаба. В [SP+1] лежит текущее приближение к обратной величине.

Как ни странно, здесь нам хватает всего 4 итераций, чтобы получить 16 точных бит результата. Немудрено - входной диапазон значений совсем мал, мы позаботились об этом...

Внутри цикла мы, по сути, вычисляем выражение


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

UFMS означает Unsigned Fused Multiply-Subtract, очередное "умножение с накоплением", но в этот раз беззнаковое.


MULU - MULtiply Unsigned. Да, у нас целый зоопарк умножений - есть знаковое, есть беззнаковое, есть смешанное. Но вроде бы в этом проглядывается некоторая извращённая логика...

Затем нас снова выручает, что регистр C (второй вход АЛУ) не "сгорает" после умножения, там по-прежнему лежит наше "предыдущее приближение", так что тут же домножаем ещё и на него, завершая итерацию.

По завершении цикла, мы заносим вычисленное значение в переменную Tx (опять на это ушло 2 слова - обидно!)

Последний этап - найти оставшиеся компоненты вектора.
Вот соответствующий код:

                    C           UAC
                    X           Rx
                    Z           Ty
                    i           1
        @@Vec:      MULSU       [X+i]
                    [Z+i]       Acc
                    iLOOP       @@Vec


Rx и идущий за ним Ry - это два нижних элемента матрицы 3х2 аффинного преобразования. Мы нашли их в части 2, а затем забыли об их существовании до этого момента. А теперь помножили их на X, и занесли в ячейки Ty, Tz.

ВСЁ! ФИНИШ!!!

Посмотрим, как оно работает. Результаты работы с дальности 300 метров были продемонстрированы в части 1. Сейчас глянем, как оно ведёт себя с 30 метров и при крене в 90 градусов.

Поставим Breakpoint на первой же строчке процедуры FindVector:


Здесь масштаб чуть побольше, 8827,344. Если бы мы считали его "по-честному" (с квадратным корнем), получилось бы 8826,849, ошибка составила 0,006%.

Переполнения пока не происходит. k=3.

Поставим Breakpoint по метке @@endloop:


Как видно по k=5, прошло 2 итерации, и действительно значение аккумулятора увеличилось вчетверо, достигнув переполнения (OFLO=1).

Наконец, переходим к концу процедуры:


Мы получили Exp=5, Tx = 60818, Ty = 2159, Tz = 349. Переведя к нормальным величинам, получим:

Tx_real = 2Exp-1 * Tx / 32768 = 29,696 метра,
Ty_real = 2Exp-2 * Ty / 32768 = 0,527 метра,
Tz_real = 2Exp-2 * Tz / 32768 = 0,085 метра


Очень похоже на правду.

Я ещё не "встраивал" этот эмулятор в свою модель сближения, надо будет это сделать и понаблюдать за работой данного алгоритма во всём диапазоне дальностей, и при различном крене, ракурсах, тангаже и курсе.


Кстати, добавил счётчик тактов в эмулятор. Оказывается, весь аффинный алгоритм целиком выполняется за 4000 тактов (это если у нас старая ПЛИС, не имеющая аппаратного перемножителя, поэтому на все операции умножения выделяем 17 тактов, а на любые другие - 1 такт). Неплохо, как мне кажется. Он должен запускаться 10 раз в секунду, что означает - 40 000 тактов за секунду, т.е если этот процессор больше ничем не будет занят, ему хватило бы тактовой частоты 40 кГц, тогда как на ПЛИС 5576ХС4Т он без проблем мог бы фурычить на 15 МГц, так что имеется запас по быстродействию в 375 раз :)

Боюсь, правда, что алгоритм сопровождения быстренько этот запас подсократит, особенно в своей наиболее полной версии с вычислением ковариационной матрицы шума измерений, и в стерео-режиме.
Но сначала надо написать код - и посмотреть...
Tags: tex, ПЛИС, кватернионы-это просто (том 1), математика, программки, работа, странные девайсы
Subscribe

  • Ремонт лыжных мостиков

    Вернулся с сегодняшнего субботника. Очень продуктивно: отремонтировали все ТРИ мостика! Правда, для этого надо было разделиться, благо народу…

  • Гетто-байк

    В субботу во время Великой Октябрьской резни бензопилой умудрился петуха сломать в велосипеде. По счастью, уже на следующий день удалось купить…

  • А всё-таки есть польза от ковариаций

    Вчера опробовал "сценарий", когда варьируем дальность от 1 метра до 11 метров. Получилось, что грамотное усреднение - это взять с огромными весами…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments