Алг. ближней дистанции, поиск "центральных" точек

Apr 27, 2021 18:12

Если помните, мишень ближней дистанции у нас трёхмерная: 6 уголковых отражателей стоят в одной плоскости, а ещё два, что по центру - вынесены вперёд:


(см. макет: да будет свет)

поэтому, в зависимости от того, с какой стороны на неё смотришь, центральные точки могут очень хорошо гулять во все стороны:

image Click to view



Основная логика пока такова: у нас ракурс согласно ТЗ не может составить более 30°, и центральная точка всё-таки ограничена в своём перемещении. По последним данным, она вынесена вперёд на 6 см (вообще данная мишень должна иметь глубину не более 9,5 см, но уголковые отражатели тоже некоторую глубину имеют, особенно в своих оправках), поэтому при повороте на 30° может сместиться на 3,5 см, тогда как 3 уголка, лежащие на плоскости, отстоят каждый на 5,5 см.

Поэтому просто ищем точку, расстояние которой до "центральной позиции" (где точка стоит при взгляде под прямым углом) минимально

"Центральные позиции" можно сколько-нибудь точно поместить на отрезок, соединяющий наиболее отдалённые точки, его мы уже находили. Если назвать эти точки P6 и P7, то координаты "центральных позиций" выйдут:

0,7914P6 + 0,2086P7 и
0,2086P6 + 0,7914P7.

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

Впрочем, компьютерное моделирование показало, что всё не так просто, и на углах вблизи 30° бывают сбои. Вот тут ещё всё правильно:



(с нумерацией просто беда. Здесь точкам даны те номера, что в техническом задании на мишень. Вот чья-то левая пятка решила дать такую нумерацию - теперь будь добр используй. Вынесенные вперёд точки имеют номера 1 и 2. Затем перечисляются точки слева, а затем точки справа)

А здесь, буквально кадром позже:



Чудеса, да и только! Поначалу я грешил на перспективу, из-за которой так легко делить отрезки на части не получается, поскольку масштаб плавно меняется.


Такой эффект безусловно присутствует на малых дистанциях, а на моделировании была довольно малая дистанция в 2 метра.


То есть, при взгляде с ракурса 30°, из-за разницы в "глубине" разных точек, мы будем высчитывать координаты центральных точек не совсем верно.

У меня такая ошибка возникала именно при большом угле пассивного курса, и удавалось проблему убрать, если добавить ещё одну "эвристику": посмотреть величину проекции на отрезок, соединяющий крайние точки, и если одна из точек прилегает слишком близко к одной из крайних, то это ЗАВЕДОМО центральная, поскольку другие так близко подойти не могли бы!

Хотел было реализовать на ассемблере обе эти эвристики, но начали терзать смутные сомнения. Решил добавить к своей компьютерной модели дополнительную "визуализацию" центров пятен, которые собственно и были переданы алгоритму. И увидел, что ровно в тех ситуациях, когда алгоритм ошибался, он делал это из-за кривого определения центров:



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

Именно из-за этого (сдвижки одной точки вправо, а второй - влево) алгоритм путался. Так что реализовывать на ассемблере дополнительную эвристику я не буду!

Надо, наконец-то взяться за код! Для начала, "рыба":

;состояние регистров к этому моменту:
;i=j=k=0
;C = Vy (y-компонента вектора, соединяющего крайние точки)
;Acc - последнее скалярное произведение (потеряло актуальность)
;X = Points2D (начало массива найденных точек)
;Y = a11 (здесь лежит вектор V, соединяющий крайние точки, а вообще это "временное хранилище")
;Z = Fx1 (сдвинуто на 1 точку отн. X)
;[SP] = @@loopEnd, адрес возврата для SwapPoints (потеряло актуальность)
;[SP+1] - 1/8 квадрата расстояния между двумя самыми дальними точками (потеряло актуальность)
FindCentralPoints proc

FindCentralPoints endp

Вспомнили, что лежит в регистрах и на стеке, вдруг пригодится.

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

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

;1. Находим 0,7914P[6] + 0,2086P[7] - координата левой центральной точки, если смотреть на мишень под прямым углом.
;(в противном случае она уползёт в сторону, но не так уж сильно)
i 6
k 1
@@k_loop: C 25933 ;число 0,7914 в формате Q1.15
MUL [X+2i+k] ;Fy6 на первой итерации, Fx6 на второй
C 6835 ;число 0,2086 в формате Q1.15
FMA [Z+2i+k] ;Fy7 на первой итерации, Fx7 на второй
[Y+k] Acc
kLOOP @@k_loop

Далее, нужно найти точку, которая к этим координатам ближе всего. Тут по "стандартной методике": в [SP+1] будем хранить минимальную дистанцию, а в начале нужно положить что-то очень большое, например 32767. [SP] будет использоваться как промежуточная переменная, чтобы хранить "текущую сумму квадратов". Индекс у нас всего один, хранить его будем в i. А циклы у нас будут по j (чтобы перебрать 3 точки) и по k (координата X/Y). Поехали:

;2. находим точку, которая к этим координатам ближе всего.
[SP+1] 32767 ;текущий минимум, инициализируем максимальным значением
;индекс самой близкой к центру точки будет храниться в j, её инициализировать не требуется
j 2 ;проверяем первые 3 точки
@@outer_cycle: k 1
[SP] 0 ;сумма квадратов
@@inner_cycle: Acc [X+2j+k] ;Fy[i] если k=1, Fx[i] если k=0
SUB [Y+k] ;Cy если k=1, Cx если k=0
SQRD2 Acc
ADD [SP]
[SP] Acc
kLOOP @@inner_cycle
;досчитали квадрат дистанции до центра, лежит как в [SP], так и в Acc, очень удобно
;проверяем, насколько он мал
SUB [SP+1]
JGE @@skip
[SP+1] [SP]
i j
@@skip: jLOOP @@outer_cycle

Итак, индекс нашли. Но теперь нужно "аккуратно" переместить эту точку на определённую позицию. Поразмыслим, что будет, если мы выберем индекс 1.

Если точка уже имела индекс 1 - всё хорошо. Можем даже "поменять её саму с собой" - время потратим, но ничего не испортим :)

Если точка имеет индекс 0, тогда можем поменять её местами с точкой под индексом 1 - и задача выполнена. То же самое, если она имела индекс 2.

Фух, всё не так страшно, как мне казалось...

Просто если бы мы выбрали в качестве "финальной позиции" индекс 0, тогда как получили индекс 2, то просто поменять их местами было нельзя, нарушился бы правильный порядок всех остальных точек, "слева направо". И я уже размышлял, как бы организовать этот цикл по протаскиванию точки аж на 2 позиции влево... Ну как размышлял - на компьютерной модели так и сделал, не мудрствуя лукаво! Очень торопился, подгоняли меня "ответь уже, сможешь по такой конфигурации мишени работать?", поэтому сделал "в лоб".

Сейчас нам удивительно везёт: можно просто вызвать SwapPoints, во всех регистрах лежит ровно то, что нам надо:

;точка с индексом i ближе всего к центру, а j=0. То, что надо!
[SP] Call(SwapPoints)

Регистр X указывает в начало массива точек, регистр Z - на одну точку дальше, поэтому пара (Z, j) при j=0 как раз указывают на точку с индексом 1.

Теперь надо придумать, как этот же самый код задействовать для возни со второй точкой, т.е нахождении "центральной" среди точек с индексами 3,4,5

Как вычислить новые координаты центра - мы уже обсуждали, достаточно поменять местами точки 6 и 7.

Далее, есть ОЧЕНЬ ТУПОЙ способ найти индекс самой близкой к центру точки - ОБА РАЗА ВЕСТИ ПОИСК СРЕДИ ПЕРВЫХ 6 ТОЧЕК! Потеряем на этом около 14 мкс, зато упрощается код :)

И наконец, менять точки местами будем вне процедуры, так удобнее. Вот, что получается в итоге:

;состояние регистров к этому моменту:
;i=j=k=0
;C = Vy (y-компонента вектора, соединяющего крайние точки)
;Acc - последнее скалярное произведение (потеряло актуальность)
;X = Points2D (начало массива найденных точек)
;Y = a11 (здесь лежит вектор V, соединяющий крайние точки, а вообще это "временное хранилище")
;Z = Fx1 (сдвинуто на 1 точку отн. X)
;[SP] = @@loopEnd, адрес возврата для SwapPoints (потеряло актуальность)
;[SP+1] - 1/8 квадрата расстояния между двумя самыми дальними точками (потеряло актуальность)
FindCentralPoints proc

;дважды вызываем процедуру FindCentralPoint, первый раз для "левой" центральной точки, второй раз для "правой"
[SP++] Call(FindCentralPoint) ;классический вызов процедуры, т.к она использует стек [SP] и [SP+1], недопустимо чтобы затёрла адрес возврата
;помещаем "левую" центральную точку в индекс 1.
[SP] Call(SwapPoints) ;вызов без инкремента стека, чтобы можно было в неё запрыгивать из условных переходов, зная что SP останется где надо
;меняем местами точки 6 и 7
; i 5
; j 5
ijk 0x00A5 ;{Inv, k, i, j} когда-нибудь надо ввести синтаксис, чтобы указывать биты don't care, чуть поможет компилятору
[SP] Call(SwapPoints)
;теперь FindCentralPoint найдёт "правую" центральную точку
[SP++] Call(FindCentralPoint)
;помещаем "правую" центральную точку в индекс 4
j 3
[SP] Call(SwapPoints)

;вот и всё. Центральные точки сидят в индексах 1 и 4, крайние - в индексах 6 и 7 (причём поменялись местами), остальные отсортированы "слева направо".
FindCentralPoints endp

endless: JMP endless

FindCentralPoint proc
;1. Находим 0,7914P[6] + 0,2086P[7] - координата левой центральной точки, если смотреть на мишень под прямым углом.
;(в противном случае она уползёт в сторону, но не так уж сильно)

; i 6
; k 1
; j 5 для цикла по поиску самой близкой к центру точки
ijk 0x04C5 ;одним махом всех иниц. {Inv, k, i, j}
@@k_loop: C 25933 ;число 0,7914 в формате Q1.15
MUL [X+2i+k] ;Fy6 на первой итерации, Fx6 на второй
C 6835 ;число 0,2086 в формате Q1.15
FMA [Z+2i+k] ;Fy7 на первой итерации, Fx7 на второй
[Y+k] Acc
kLOOP @@k_loop

;2. находим точку, которая к этим координатам ближе всего.
[SP+1] 32767 ;текущий минимум, инициализируем максимальным значением
;индекс самой близкой к центру точки будет храниться в i, её инициализировать не требуется
@@outer_cycle: k 1
[SP] 0 ;сумма квадратов
@@inner_cycle: Acc [X+2j+k] ;Fy[i] если k=1, Fx[i] если k=0
SUB [Y+k] ;Cy если k=1, Cx если k=0
SQRD2 Acc
ADD [SP]
[SP] Acc
kLOOP @@inner_cycle
;досчитали квадрат дистанции до центра, лежит как в [SP], так и в Acc, очень удобно
;проверяем, насколько он мал
SUB [SP+1]
JGE @@skip
[SP+1] [SP]
i j
@@skip: jLOOP @@outer_cycle
;точка с индексом i ближе всего к центру, а j=0. То, что надо!
JMP [--SP] ;ret
FindCentralPoint endp

на этот этап требуется аж 30 слов кода, или 60 байт. Всего "алгоритм ближней дистанции" пока что занимает 81 слово кода, или 162 байта. И кто-то после этого скажет, что ассемблер - не выразительный, очень громоздкий язык!?

Осталось это дело проверить, а потом ещё один кусочек - "случай Берегового".

странные девайсы, ПЛИС, программки, работа

Previous post Next post
Up