Ликбез по кватернионам, часть 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
У данной задачи много имён. В английской литературе встретил Absolute orientation, Point set registration, а также Point cloud registration, scan matching.
Суть вот в чём: имеется твёрдое тело, на котором жёстко расположено N точек. Введена некая система координат, также жёстко связанная с этим твёрдым телом, и даны координаты N точек в этой системе, rk (rkx,rky,rkz), от слова reference - "эталонный".
А далее мы измерили координаты этих N точек в другой системе координат, mk (mkx,mky,mkz), от слова measured - "измеренный".
Задача состоит в том, чтобы определить параллельный перенос между началами координат, поворот из одной системы координат в другую, и, возможно, масштабирующий множитель, такие, при которых два набора координат точек согласуются наилучшим образом.
Я столкнулся с этой задачей в связи с юстировкой прибора, с использованием лазерного трекера. Лазерный трекер - это "адская смесь" дальномера и теодолита: наводишь его на определённую точку, он измеряет два угла (азимут и угол места), дальность и автоматически преобразует это в трёхмерные координаты точки, получая точность лучше десятых долей миллиметра. С его помощью можно узнать координаты всех моих уголковых отражателей, а потом координаты неких характерных точек моего прибора, и затем, дважды решив рассмотренную здесь задачу - узнать, что должен показать мой прибор, будучи идеально откалиброванным.
Подозреваю, что где-то в программном обеспечении для лазерного трекера данная задача давным-давно "зашита", вместе с решением, но точно сказать не могу - "зависит от комплектации", а приобретать такой приборчик себе точно не буду по причине его запредельной цены. Пока знаю лишь, что такой наличествует у РКК Энергия, и нынче они практически отказались от более классических измерительных приборов вроде автотеодолита и плоских зеркал, наклеиваемых на различные части космического аппарата.
Ну и для ориентации в пространстве некоего автономного агрегата, имеющего на борту сканирующий лидар или прочие штучки, позволяющие напрямую получать трёхмерные координаты окружающих объектов, данная задача абсолютно необходима!
Математическая формулировка задачи
Дано N "эталонных" векторов rk, k=1..N и N "измеренных" векторов mk. Соответствие между точками известно! То есть точка под номером k имеет "эталонные" координаты rk и "измеренные" mk и никак иначе.
Необходимо найти такой вектор t (от слова translation - параллельный перенос), число s (scale - масштаб) и поворот R() (его мы пока записываем как преобразование, а как именно он выражен - матрицей, кватернионом, углами Эйлера и пр. - пока не важно), чтобы выражения
+\vec{\mathbf{t}})
выполнялись как можно точнее. Можно потребовать, чтобы сумма квадратов ошибок,
-\vec{\mathbf{t}}\right|^2)
была минимальной, это приведёт к наиболее простым результатам.
Взятие центроидов
Но как бы мы не формулировали критерий оптимизации, оказывается очень полезно найти среднее арифметическое всех векторов в одной и в другой системе координат, так называемые центроиды:
А также "передвинем начало координат" в эти центроиды, т.е введём новые вектора:
(Приглашается 2born рассказать, как заставить TeX не писать треугольные скобки настолько "врастопырку". UPD. Исправил, СПАСИБО!)
Старые выразятся через новые следующим образом:
Подставим эти выражения в формулу для Θ, которую мы хотим минимизировать:
-\vec{\mathbf{t}}|^2)
Мы знаем, что оператор поворота - линейный, поэтому поворот от суммы векторов равен сумме векторов, повёрнутых "по отдельности":
-sR(\left<\vec{\mathbf{r}}\right>)-\vec{\mathbf{t}}|^2)
Введём новый вектор t':
-\left<\vec{\mathbf{m}}\right>)
С ним, выражение для Θ упростится:
-\vec{\mathbf{t%27}}|^2)
Раскроем квадрат модуля разности, приняв первые 2 слагаемых "за единое целое":
|^2+N|\vec{\mathbf{t%27}}|^2-2\left(\sum_{k=1}^N{(\vec{\mathbf{m%27_k}}-sR(\vec{\mathbf{r%27_k}}))},\vec{\mathbf{t%27}}\right))
Правое слагаемое - это скалярное произведение. Вектор в левой его части равен НУЛЮ, поскольку новые вектора m'k и r'k взяты "относительно среднего значения", т.е сумма что одних, что других будет нулевой. Благодаря этому у нас остаётся только два слагаемых:
|^2+N|\vec{\mathbf{t%27}}|^2)
И только правое зависит от вектора t. Поэтому мы можем найти данный вектор, минимизируя правое слагаемое, а именно, приравняв его нулю:
-\left<\vec{\mathbf{m}}\right>=0,)
откуда:
)
Ответ выражается через два усреднённых вектора (центроида) и пока ещё неизвестные нам масштаб и поворот. Но смысл достаточно ясный: как только мы узнаем поворот и масштаб - параллельный перенос найдём как разницу между усреднёнными векторами.
А минимизировать мы теперь должны левое слагаемое:
|^2)
Нахождение масштаба
Раскроем в этом выражении квадрат модуля:
\right)+s^2\sum_{k=1}^N{|R(\vec{\mathbf{r%27_k}})|^2})
В третьем слагаемом можно убрать поворот, взяв просто квадраты длин векторов rk, поскольку поворот не меняет длины. И далее, введём обозначения:
\right),)
Всё это скаляры: сумма квадратов длин измеренных векторов, сумма скалярных произведений одних на повёрнутые другие, и сумма квадратов длин эталонных векторов. S - от слова sum, D - от слова Dot product (скалярное произведение).
Тогда величина, которую мы пытаемся минимизировать, выражается так:
причём величины Sm, D и Sr не зависят от масштаба s. D зависит от выбранного поворота, но какой бы поворот мы ни выбрали, далее мы можем минимизировать Θ, правильно подобрав s. А именно: Sr>0, то есть у нас получается выпуклая парабола. Точка минимума параболы ax2+bx+c - это -b/2a. В нашем случае:
\right)}}{\sum_{k=1}^N{|\vec{\mathbf{r%27_k}}|^2}})
Чтобы найти масштаб - нужно сначала найти поворот (мы его пока не знаем), а затем, зная поворот и масштаб - найдём параллельный перенос.
Если теперь подставить s в Θ, получим:
Причём Sr и Sm константы, зависящие только от исходных векторов. От поворота зависит только D. Поэтому остаётся лишь максимизировать D.
Более "симметричный" масштаб
В [данной работе] (которую я тут и излагаю немного по-своему) очень беспокоятся, что если посчитать масштаб по выражению выше, пропадает симметрия между rk и mk: если мы поменяем их местами, то могли бы ожидать вектор параллельного переноса со знаком "минус", обратное значение масштаба и обратный поворот - это было бы логично. Но, вообще говоря, не получаем.
Но если, после нахождения параллельного переноса постараться минимизировать вот такую величину:
\right|^2,)
то получим следующее выражение:
Чтобы найти минимум, проще всего взять производную и приравнять к нулю:
Очень любопытный результат:
- теперь, чтобы найти масштаб, нам необязательно искать поворот - ответ уже выражается только через входные векторы. По сути, "дисперсию измеренных векторов поделить на дисперсию эталонных - и взять квадратный корень".
- очевидно, что если rk и mk поменять местами, то получим попросту обратную величину масштаба, что логично.
А чтобы найти поворот, надо, как и в прошлом случае, максимизировать D.
Нахождение поворота
Итак, нам надо найти максимум выражения D:
\right))
Сумма скалярных произведений измеренных векторов с повёрнутыми эталонными должна оказаться максимальной.
Но ведь мы эту задачу уже решали, см. часть 12 - навигационная задача. Там мы начали с "задачи Вахбы", минимизации суммы квадратов ошибок (но там ничего, кроме поворота, и не надо было находить) - и очень быстро привели эту задачу к максимизации суммы скалярных произведений!
Далее в рассмотренной нами работе по сути повторяют результаты Дэвенпорта (см. Дэвенпорт берёт след!), попутно вводя кватернионы и их основные свойства, в количестве, достаточном, чтобы прийти к результату. Напомним: из эталонных и измеренных векторов неким довольно хитрым образом составляется симметричная матрица 4х4, находят её максимальное собственное значение и соответствующий собственный вектор из 4 компонент. Этот "вектор" и будет кватернионом искомого поворота, останется лишь его отнормировать (или изначально находить его с дополнительным условием единичной нормы).
Но я полагаю, что с тем же успехом можно применить линейный метод Мортари-Маркли. Он решает чуть другую задачу (по-другому сформулирован критерий "близости" двух векторов на плоскости) и обладает "особыми точками" - если имеем дело с поворотом на 180°, то придём к вырожденной системе линейных уравнений. Вблизи 180° решение будет, но все коэффициенты уравнений существенно уменьшатся, что может повлечь численную неустойчивость. Но если мы заранее знаем, что угол поворота далёк от 180°, то метод должен отработать на "пять с плюсом". Всё-таки, решить систему из 3 линейных уравнений с 3 неизвестными - немного приятнее, чем искать собственный вектор матрицы 4х4, соответствующий максимальному собственному значению!
Подытожим.
Получив две группы векторов - эталонные rk и измеренные mk, мы:
1. Находим центроиды, т.е среднее арифметическое первой и второй группы векторов:
2. Центрируем векторы относительно их средних:
Исходные векторы больше не нужны.
3. Решаем навигационную задачу для эталонных векторов r'k и измеренных m'k (отцентрированных) любым понравившимся способом: через матрицу Дэвенпорта, линейным методом Мортари-Маркли или как-нибудь ещё, результатом становится поворот в любой удобной нам форме.
4. Находим масштаб s по одной из двух приведённых здесь формул. Наверное, лучше взять вторую:
она не зависит от поворота и симметрична, если эталонные и измеренные векторы поменять местами. А можно и вовсе постановить масштаб единичным, если мы знаем, что масштабирование возникать не может.
5. Зная поворот и масштаб, находим параллельный перенос:
В следующей части (которая в книге "уползёт" в упражнения и ответы к ним) попробую всё это применить "на практике".
[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
У данной задачи много имён. В английской литературе встретил Absolute orientation, Point set registration, а также Point cloud registration, scan matching.
Суть вот в чём: имеется твёрдое тело, на котором жёстко расположено N точек. Введена некая система координат, также жёстко связанная с этим твёрдым телом, и даны координаты N точек в этой системе, rk (rkx,rky,rkz), от слова reference - "эталонный".
А далее мы измерили координаты этих N точек в другой системе координат, mk (mkx,mky,mkz), от слова measured - "измеренный".
Задача состоит в том, чтобы определить параллельный перенос между началами координат, поворот из одной системы координат в другую, и, возможно, масштабирующий множитель, такие, при которых два набора координат точек согласуются наилучшим образом.
Я столкнулся с этой задачей в связи с юстировкой прибора, с использованием лазерного трекера. Лазерный трекер - это "адская смесь" дальномера и теодолита: наводишь его на определённую точку, он измеряет два угла (азимут и угол места), дальность и автоматически преобразует это в трёхмерные координаты точки, получая точность лучше десятых долей миллиметра. С его помощью можно узнать координаты всех моих уголковых отражателей, а потом координаты неких характерных точек моего прибора, и затем, дважды решив рассмотренную здесь задачу - узнать, что должен показать мой прибор, будучи идеально откалиброванным.
Подозреваю, что где-то в программном обеспечении для лазерного трекера данная задача давным-давно "зашита", вместе с решением, но точно сказать не могу - "зависит от комплектации", а приобретать такой приборчик себе точно не буду по причине его запредельной цены. Пока знаю лишь, что такой наличествует у РКК Энергия, и нынче они практически отказались от более классических измерительных приборов вроде автотеодолита и плоских зеркал, наклеиваемых на различные части космического аппарата.
Ну и для ориентации в пространстве некоего автономного агрегата, имеющего на борту сканирующий лидар или прочие штучки, позволяющие напрямую получать трёхмерные координаты окружающих объектов, данная задача абсолютно необходима!
Математическая формулировка задачи
Дано N "эталонных" векторов rk, k=1..N и N "измеренных" векторов mk. Соответствие между точками известно! То есть точка под номером k имеет "эталонные" координаты rk и "измеренные" mk и никак иначе.
Необходимо найти такой вектор t (от слова translation - параллельный перенос), число s (scale - масштаб) и поворот R() (его мы пока записываем как преобразование, а как именно он выражен - матрицей, кватернионом, углами Эйлера и пр. - пока не важно), чтобы выражения
выполнялись как можно точнее. Можно потребовать, чтобы сумма квадратов ошибок,
была минимальной, это приведёт к наиболее простым результатам.
Взятие центроидов
Но как бы мы не формулировали критерий оптимизации, оказывается очень полезно найти среднее арифметическое всех векторов в одной и в другой системе координат, так называемые центроиды:
А также "передвинем начало координат" в эти центроиды, т.е введём новые вектора:
(Приглашается 2born рассказать, как заставить TeX не писать треугольные скобки настолько "врастопырку". UPD. Исправил, СПАСИБО!)
Старые выразятся через новые следующим образом:
Подставим эти выражения в формулу для Θ, которую мы хотим минимизировать:
Мы знаем, что оператор поворота - линейный, поэтому поворот от суммы векторов равен сумме векторов, повёрнутых "по отдельности":
Введём новый вектор t':
С ним, выражение для Θ упростится:
Раскроем квадрат модуля разности, приняв первые 2 слагаемых "за единое целое":
Правое слагаемое - это скалярное произведение. Вектор в левой его части равен НУЛЮ, поскольку новые вектора m'k и r'k взяты "относительно среднего значения", т.е сумма что одних, что других будет нулевой. Благодаря этому у нас остаётся только два слагаемых:
И только правое зависит от вектора t. Поэтому мы можем найти данный вектор, минимизируя правое слагаемое, а именно, приравняв его нулю:
откуда:
Ответ выражается через два усреднённых вектора (центроида) и пока ещё неизвестные нам масштаб и поворот. Но смысл достаточно ясный: как только мы узнаем поворот и масштаб - параллельный перенос найдём как разницу между усреднёнными векторами.
А минимизировать мы теперь должны левое слагаемое:
Нахождение масштаба
Раскроем в этом выражении квадрат модуля:
В третьем слагаемом можно убрать поворот, взяв просто квадраты длин векторов rk, поскольку поворот не меняет длины. И далее, введём обозначения:
Всё это скаляры: сумма квадратов длин измеренных векторов, сумма скалярных произведений одних на повёрнутые другие, и сумма квадратов длин эталонных векторов. S - от слова sum, D - от слова Dot product (скалярное произведение).
Тогда величина, которую мы пытаемся минимизировать, выражается так:
причём величины Sm, D и Sr не зависят от масштаба s. D зависит от выбранного поворота, но какой бы поворот мы ни выбрали, далее мы можем минимизировать Θ, правильно подобрав s. А именно: Sr>0, то есть у нас получается выпуклая парабола. Точка минимума параболы ax2+bx+c - это -b/2a. В нашем случае:
Чтобы найти масштаб - нужно сначала найти поворот (мы его пока не знаем), а затем, зная поворот и масштаб - найдём параллельный перенос.
Если теперь подставить s в Θ, получим:
Причём Sr и Sm константы, зависящие только от исходных векторов. От поворота зависит только D. Поэтому остаётся лишь максимизировать D.
Более "симметричный" масштаб
В [данной работе] (которую я тут и излагаю немного по-своему) очень беспокоятся, что если посчитать масштаб по выражению выше, пропадает симметрия между rk и mk: если мы поменяем их местами, то могли бы ожидать вектор параллельного переноса со знаком "минус", обратное значение масштаба и обратный поворот - это было бы логично. Но, вообще говоря, не получаем.
Но если, после нахождения параллельного переноса постараться минимизировать вот такую величину:
то получим следующее выражение:
Чтобы найти минимум, проще всего взять производную и приравнять к нулю:
Очень любопытный результат:
- теперь, чтобы найти масштаб, нам необязательно искать поворот - ответ уже выражается только через входные векторы. По сути, "дисперсию измеренных векторов поделить на дисперсию эталонных - и взять квадратный корень".
- очевидно, что если rk и mk поменять местами, то получим попросту обратную величину масштаба, что логично.
А чтобы найти поворот, надо, как и в прошлом случае, максимизировать D.
Нахождение поворота
Итак, нам надо найти максимум выражения D:
Сумма скалярных произведений измеренных векторов с повёрнутыми эталонными должна оказаться максимальной.
Но ведь мы эту задачу уже решали, см. часть 12 - навигационная задача. Там мы начали с "задачи Вахбы", минимизации суммы квадратов ошибок (но там ничего, кроме поворота, и не надо было находить) - и очень быстро привели эту задачу к максимизации суммы скалярных произведений!
Далее в рассмотренной нами работе по сути повторяют результаты Дэвенпорта (см. Дэвенпорт берёт след!), попутно вводя кватернионы и их основные свойства, в количестве, достаточном, чтобы прийти к результату. Напомним: из эталонных и измеренных векторов неким довольно хитрым образом составляется симметричная матрица 4х4, находят её максимальное собственное значение и соответствующий собственный вектор из 4 компонент. Этот "вектор" и будет кватернионом искомого поворота, останется лишь его отнормировать (или изначально находить его с дополнительным условием единичной нормы).
Но я полагаю, что с тем же успехом можно применить линейный метод Мортари-Маркли. Он решает чуть другую задачу (по-другому сформулирован критерий "близости" двух векторов на плоскости) и обладает "особыми точками" - если имеем дело с поворотом на 180°, то придём к вырожденной системе линейных уравнений. Вблизи 180° решение будет, но все коэффициенты уравнений существенно уменьшатся, что может повлечь численную неустойчивость. Но если мы заранее знаем, что угол поворота далёк от 180°, то метод должен отработать на "пять с плюсом". Всё-таки, решить систему из 3 линейных уравнений с 3 неизвестными - немного приятнее, чем искать собственный вектор матрицы 4х4, соответствующий максимальному собственному значению!
Подытожим.
Получив две группы векторов - эталонные rk и измеренные mk, мы:
1. Находим центроиды, т.е среднее арифметическое первой и второй группы векторов:
2. Центрируем векторы относительно их средних:
Исходные векторы больше не нужны.
3. Решаем навигационную задачу для эталонных векторов r'k и измеренных m'k (отцентрированных) любым понравившимся способом: через матрицу Дэвенпорта, линейным методом Мортари-Маркли или как-нибудь ещё, результатом становится поворот в любой удобной нам форме.
4. Находим масштаб s по одной из двух приведённых здесь формул. Наверное, лучше взять вторую:
она не зависит от поворота и симметрична, если эталонные и измеренные векторы поменять местами. А можно и вовсе постановить масштаб единичным, если мы знаем, что масштабирование возникать не может.
5. Зная поворот и масштаб, находим параллельный перенос:
В следующей части (которая в книге "уползёт" в упражнения и ответы к ним) попробую всё это применить "на практике".