nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

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

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

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


Основная логика пока такова: у нас ракурс согласно ТЗ не может составить более 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 байта. И кто-то после этого скажет, что ассемблер - не выразительный, очень громоздкий язык!?

Осталось это дело проверить, а потом ещё один кусочек - "случай Берегового".
Tags: ПЛИС, программки, работа, странные девайсы
Subscribe

  • Тестируем atan1 на QuatCore

    Пора уже перебираться на "железо" потихоньку. Решил начать с самого первого алгоритма, поскольку он уже был написан на ассемблере. В программу внёс…

  • Формулы приведения, что б их... (и atan на ТРЁХ умножениях)

    Формулу арктангенса на 4 умножениях ещё немножко оптимизировал с помощью алгоритма Ремеза: Ошибка уменьшилась с 4,9 до 4,65 угловой секунды, и…

  • Алгоритм Ремеза в экселе

    Вот и до него руки дошли, причина станет ясна в следующем посте. Изучать чужие библиотеки было лениво (в том же BOOSTе сам чёрт ногу сломит), писать…

  • atan на ЧЕТЫРЁХ умножениях

    Мишка такой человек — ему обязательно надо, чтоб от всего была польза. Когда у него бывают лишние деньги, он идёт в магазин и покупает какую-нибудь…

  • Ай да Пафнутий Львович!

    Решил ещё немного поковыряться со своим арктангенсом. Хотел применить алгоритм Ремеза, но начал с узлов Чебышёва. И для начала со своего "линейного…

  • atan(y/x) на двух умножениях!

    Чего-то никак меня не отпустит эта тема, всё кажется, что есть очень простой и эффективный метод, надо только его найти! Сейчас вот такое…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments