Ликбез по кватернионам, задачка к части 17
Оглавление "Ликбеза по кватернионам":
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3
Хочется опробовать это дело на более-менее реальных данных. Имеется у нас такая вот мишень:
ну, в реальности вместо светоотражающей плёнки будут уголковые отражатели из кварцевого стекла, а вместо ДСП - алюминиевый сплав АМг6. Не знаю, почему именно его у нас так любят - заклёпками, сваркой, штамповкой не балуемся, исключительно всё выфрезеровываем из цельного куска металла, вроде есть и более прочные и лёгкие для таких случаев. Слышал, что он лучше переносит вибрации и удары - не такой хрупкий, как другие. Может, и так. А скорее всего, просто, как начали - так зачем чего-то менять.
Так вот, координаты центров отражателей в "эталонной" системе координат. Её начало расположено примерно там, где пересекаются две карандашные линии. "На нас" (перпендикулярно доске) - ось X, "слева направо" - ось Y, "снизу вверх" - ось Z. Примерно так:
r1 (60; -104; 0),
r2 (60; 104; 0),
r3 (0; -69; 43),
r4 (0; -84; -51),
r5 (0; -158; 9),
r6 (0; 158; 9),
r7 (0; 69; 43),
r8 (0; 84; -51)
координаты в миллиметрах, без десятых и сотых, для простоты. И немного не соответствует макету (уголки чуть-чуть передвинулись), чтобы сбить с толку вероятного противника.
И давайте мы будем эту хреновину промерять "сбоку":
Лазерный трекер расположится чуть слева. И допустим, мы получили следующие координаты этих 8 точек:
m1 (-1104,1; 439,8; -0,6),
m2 (-896,1; 440,9; 0,5),
m3 (-1068,7; 499; 42,6),
m4 (-1083,4; 500,8; -50,2),
m5 (-1158,7; 500,3; 9,9),
m6 (-841,1; 499; 8,3),
m7 (-931,1; 500,5; 42,8),
m8 (-915,3; 500,7; -50,4)
Тут мы ещё ввели ошибку плюс-минус миллиметр, с округлением до десятых. Да, это очень хреновенький трекер, зато посмотрим, насколько справляется наша математика с усреднением ошибок за счёт большой выборки.
Под катом решение - чему равен поворот, масштаб и параллельный перенос. Если кому хочется попробовать самостоятельно - под кат пока не заглядывайте :)
1. нахождение средних векторов, "центроидов":
Получаем
,)
)
В первом случае, не округляй мы до целых миллиметров, вообще должны были получить (15;0;0). Ну да ладно.
2. центрирование векторов относительно средних:
r'1 (45; -104; -0,25),
r'2 (45; 104; -0,25),
r'3 (-15; -69; 42,75),
r'4 (-15; -84; -51,25),
r'5 (-15; -158; 8,75),
r'6 (-15; 158; 8,75),
r'7 (-15; 69; 42,75),
r'8 (-15; 84; -51,25)
m'1 (-104,125; -45,425; -0,9625),
m'2 (103,875; -44,225; 0,1375),
m'3 (-68,725; 13,875; 42,2375),
m'4 (-83,425; 15,675; -50,5625),
m'5 (-158,725; 15,175; 9,5375),
m'6 (158,875; 13,875; 7,9375),
m'7 (68,875; 15,375; 42,4375),
m'8 (83,375; 15,675; -50,7625)
3. Решаем навигационную задачу
Воспользуемся линейным методом Мортари-Маркли.
3.1. Строим суммы и разности векторов:
s1 (-59,125; -149,425; -1,2125),
s2 (148,875; 59,775; -0,1125),
s3 (-83,725; -55,125; 84,9875),
s4 (-98,425; -68,325; -101,8125),
s5 (-173,725; -142,825; 18,2875),
s6 (143,875; 171,875; 16,6875),
s7 (53,875; 84,375; 85,1875),
s8 (68,375; 99,675; -102,0125)
d1 (149,125; -58,575; 0,7125),
d2 (-58,875; 148,225; -0,3875),
d3 (53,725; -82,875; 0,5125),
d4 (68,425; -99,675; -0,6875),
d5 (143,725; -173,175; -0,7875),
d6 (-173,875; 144,125; 0,8125),
d7 (-83,875; 53,625; 0,3125),
d8 (-98,375; 68,325; -0,4875)
3.2. Вычисляем вектор v и матрицу M:
Веса ωn мы берём все единичные. Вектор v в таком случае - просто сумма попарных векторных произведений,
v (123,5; -119,7; 201394,8)
Компонента z превышает x,y более чем в 1000 раз! То есть, метод уже практически "догадался", что поворот был осуществлён вокруг оси Z. Но надо ещё выразить его в правильных единицах и устранить "перекрёстные связи", для чего и нужна матрица. Мы уже выписывали в явном виде произведение двух матриц внутри суммы:
)
Матрица всегда симметричная, поэтому из 9 компонент достаточно вычислять и суммировать 6.
В итоге получаем:
)
3.3 Решаем систему линейных уравнений:
Получаем
θ (0,004400749; 0,005808585; -0,999901563)
3.4 Находим кватернион поворота
Ненормированный кватернион получается "прибавлением единички", т.е θ - это наша векторная / мнимая часть кватерниона, а единица - скалярная:
Λненорм = 1 + 0,004400749i + 0,005808585j -0,999901563k
А после нормировки выходит:
Λ = 0,70713 + 0,00311i + 0,00411j -0,70706k.
Правильным ответом был бы поворот на -90° вокруг оси Z, т.е ПРОТИВ ЧАСОВОЙ СТРЕЛКИ, если смотреть в ту сторону, куда "направлена стрелочка" оси Z:
Если посчитать "расстояние" между двумя этими кватернионами с помощью метрики, получим 0,59° - такова ошибка наших измерений, "суммарно" по всем трём осям.
Если мы "уберём" шум - ответ получится точным, что, в общем-то, логично.
4. Измерение масштаба:
Получаем s=1,000114392
Когда мы моделировали эти измерения, мы считали, что масштаб единичный. То есть, в оценке масштаба ошиблись на 0,011%, не так уж и плохо. Но если мы априори знаем, что масштабирования происходить не будет, координаты могут отличаться лишь параллельным переносом и поворотом, то лучше вообще отказаться от оценки масштаба и сразу постановить s=1. Это даст чуть лучшую оценку параллельного переноса, а на оценку поворота никоим образом не повлияет.
5. Измерение параллельного переноса:
)
Подставив найденный нами поворот и масштаб, получаем
t (-999,977; 500,1285; 0,265647)
Неплохо, ведь правильный ответ: (-1000; 500; 0). Усреднение по 8 точкам явно нам помогло снизить ошибку с 1 мм (такую ошибку по модели мы внесли в измерение каждой координаты каждой точки) до менее 0,266.
А ведь работает алгоритм! Прямо-таки чудес не совершает, ошибка в 1 мм это ошибка в 1 мм, а точек здесь не сотни и не тысячи, а всего 8, а избыточности и того меньше. Просто чтобы эта задача однозначно решалась, нужно хотя бы 3 точки. Но всю имеющуюся информацию алгоритм использует по-максимуму!
Для меня практически боевое крещение линейного метода Мортари-Маркли! Я про него написал, тщательно все выкладки провёл, но ни разу не пробовал применить. Работает, и даже со знаками всё правильно. По-хорошему его ещё стоило бы сравнить с методом Дэвенпорта. Я подозреваю, что ответы будут практически идентичными, но одно дело подозревать - другое увидеть воочию.
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3
Хочется опробовать это дело на более-менее реальных данных. Имеется у нас такая вот мишень:
ну, в реальности вместо светоотражающей плёнки будут уголковые отражатели из кварцевого стекла, а вместо ДСП - алюминиевый сплав АМг6. Не знаю, почему именно его у нас так любят - заклёпками, сваркой, штамповкой не балуемся, исключительно всё выфрезеровываем из цельного куска металла, вроде есть и более прочные и лёгкие для таких случаев. Слышал, что он лучше переносит вибрации и удары - не такой хрупкий, как другие. Может, и так. А скорее всего, просто, как начали - так зачем чего-то менять.
Так вот, координаты центров отражателей в "эталонной" системе координат. Её начало расположено примерно там, где пересекаются две карандашные линии. "На нас" (перпендикулярно доске) - ось X, "слева направо" - ось Y, "снизу вверх" - ось Z. Примерно так:
r1 (60; -104; 0),
r2 (60; 104; 0),
r3 (0; -69; 43),
r4 (0; -84; -51),
r5 (0; -158; 9),
r6 (0; 158; 9),
r7 (0; 69; 43),
r8 (0; 84; -51)
координаты в миллиметрах, без десятых и сотых, для простоты. И немного не соответствует макету (уголки чуть-чуть передвинулись), чтобы сбить с толку вероятного противника.
И давайте мы будем эту хреновину промерять "сбоку":
Лазерный трекер расположится чуть слева. И допустим, мы получили следующие координаты этих 8 точек:
m1 (-1104,1; 439,8; -0,6),
m2 (-896,1; 440,9; 0,5),
m3 (-1068,7; 499; 42,6),
m4 (-1083,4; 500,8; -50,2),
m5 (-1158,7; 500,3; 9,9),
m6 (-841,1; 499; 8,3),
m7 (-931,1; 500,5; 42,8),
m8 (-915,3; 500,7; -50,4)
Тут мы ещё ввели ошибку плюс-минус миллиметр, с округлением до десятых. Да, это очень хреновенький трекер, зато посмотрим, насколько справляется наша математика с усреднением ошибок за счёт большой выборки.
Под катом решение - чему равен поворот, масштаб и параллельный перенос. Если кому хочется попробовать самостоятельно - под кат пока не заглядывайте :)
1. нахождение средних векторов, "центроидов":
Получаем
В первом случае, не округляй мы до целых миллиметров, вообще должны были получить (15;0;0). Ну да ладно.
2. центрирование векторов относительно средних:
r'1 (45; -104; -0,25),
r'2 (45; 104; -0,25),
r'3 (-15; -69; 42,75),
r'4 (-15; -84; -51,25),
r'5 (-15; -158; 8,75),
r'6 (-15; 158; 8,75),
r'7 (-15; 69; 42,75),
r'8 (-15; 84; -51,25)
m'1 (-104,125; -45,425; -0,9625),
m'2 (103,875; -44,225; 0,1375),
m'3 (-68,725; 13,875; 42,2375),
m'4 (-83,425; 15,675; -50,5625),
m'5 (-158,725; 15,175; 9,5375),
m'6 (158,875; 13,875; 7,9375),
m'7 (68,875; 15,375; 42,4375),
m'8 (83,375; 15,675; -50,7625)
3. Решаем навигационную задачу
Воспользуемся линейным методом Мортари-Маркли.
3.1. Строим суммы и разности векторов:
s1 (-59,125; -149,425; -1,2125),
s2 (148,875; 59,775; -0,1125),
s3 (-83,725; -55,125; 84,9875),
s4 (-98,425; -68,325; -101,8125),
s5 (-173,725; -142,825; 18,2875),
s6 (143,875; 171,875; 16,6875),
s7 (53,875; 84,375; 85,1875),
s8 (68,375; 99,675; -102,0125)
d1 (149,125; -58,575; 0,7125),
d2 (-58,875; 148,225; -0,3875),
d3 (53,725; -82,875; 0,5125),
d4 (68,425; -99,675; -0,6875),
d5 (143,725; -173,175; -0,7875),
d6 (-173,875; 144,125; 0,8125),
d7 (-83,875; 53,625; 0,3125),
d8 (-98,375; 68,325; -0,4875)
3.2. Вычисляем вектор v и матрицу M:
Веса ωn мы берём все единичные. Вектор v в таком случае - просто сумма попарных векторных произведений,
v (123,5; -119,7; 201394,8)
Компонента z превышает x,y более чем в 1000 раз! То есть, метод уже практически "догадался", что поворот был осуществлён вокруг оси Z. Но надо ещё выразить его в правильных единицах и устранить "перекрёстные связи", для чего и нужна матрица. Мы уже выписывали в явном виде произведение двух матриц внутри суммы:
Матрица всегда симметричная, поэтому из 9 компонент достаточно вычислять и суммировать 6.
В итоге получаем:
3.3 Решаем систему линейных уравнений:
Получаем
θ (0,004400749; 0,005808585; -0,999901563)
3.4 Находим кватернион поворота
Ненормированный кватернион получается "прибавлением единички", т.е θ - это наша векторная / мнимая часть кватерниона, а единица - скалярная:
Λненорм = 1 + 0,004400749i + 0,005808585j -0,999901563k
А после нормировки выходит:
Λ = 0,70713 + 0,00311i + 0,00411j -0,70706k.
Правильным ответом был бы поворот на -90° вокруг оси Z, т.е ПРОТИВ ЧАСОВОЙ СТРЕЛКИ, если смотреть в ту сторону, куда "направлена стрелочка" оси Z:
Если посчитать "расстояние" между двумя этими кватернионами с помощью метрики, получим 0,59° - такова ошибка наших измерений, "суммарно" по всем трём осям.
Если мы "уберём" шум - ответ получится точным, что, в общем-то, логично.
4. Измерение масштаба:
Получаем s=1,000114392
Когда мы моделировали эти измерения, мы считали, что масштаб единичный. То есть, в оценке масштаба ошиблись на 0,011%, не так уж и плохо. Но если мы априори знаем, что масштабирования происходить не будет, координаты могут отличаться лишь параллельным переносом и поворотом, то лучше вообще отказаться от оценки масштаба и сразу постановить s=1. Это даст чуть лучшую оценку параллельного переноса, а на оценку поворота никоим образом не повлияет.
5. Измерение параллельного переноса:
Подставив найденный нами поворот и масштаб, получаем
t (-999,977; 500,1285; 0,265647)
Неплохо, ведь правильный ответ: (-1000; 500; 0). Усреднение по 8 точкам явно нам помогло снизить ошибку с 1 мм (такую ошибку по модели мы внесли в измерение каждой координаты каждой точки) до менее 0,266.
А ведь работает алгоритм! Прямо-таки чудес не совершает, ошибка в 1 мм это ошибка в 1 мм, а точек здесь не сотни и не тысячи, а всего 8, а избыточности и того меньше. Просто чтобы эта задача однозначно решалась, нужно хотя бы 3 точки. Но всю имеющуюся информацию алгоритм использует по-максимуму!
Для меня практически боевое крещение линейного метода Мортари-Маркли! Я про него написал, тщательно все выкладки провёл, но ни разу не пробовал применить. Работает, и даже со знаками всё правильно. По-хорошему его ещё стоило бы сравнить с методом Дэвенпорта. Я подозреваю, что ответы будут практически идентичными, но одно дело подозревать - другое увидеть воочию.