Введение в Visual SLAM ч. 2: Движение твёрдого тела

Это вторая статья из серии о Visual SLAM. Первую статью серии можно найти здесь ^[1].

Во второй части серии мы поговорим о движении твёрдого тела (в нашем случае робота) и его позиции в пространстве. В статье будет немного математики. Куда уж без неё в робототехнике. Кому интересно прошу под кат.

Во время выполнения визуального SLAM робот постоянно перемещается в пространстве. Для того чтобы определить его текущую позицию в пространстве необходимо знать его начальную позицию и перемещение в пространстве на данный момент времени. Для этого необходимо понимать как работают трансформации.

Трансформации

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

Робот обычно состоит из некоторого числа взаимосвязанных физических компонентов таких как основа, суставы, манипуляторы и сенсоры. Эти компоненты разнесены между собой на некоторое расстояние в пространстве и каждая из них имеет собственную систему координат. Чтобы локализация робота было более точной нужно понимать как преобразовать позицию объекта определенную относительно камеры (те. из системы координат камеры) к системе координат основы робота (конечная цель локализации робота в мире). У меня уже была статья ^[2]о трансформациях в ROS на Хабре.

Позицию робота в пространстве в 3D можно описать с помощью двух параметров - координаты (x, y, z) и ориентации (три угла относительно систем координат x, y и z).

Для удобства вычисления вводятся однородные координаты - к стандартным координатам x, y и z добавляется ещё одна компонента w:

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

Существует несколько классов проективных геометрических трансформаций, которые отличаются тем какие параметры объекта сохраняются при трансформации (н-р, длины сторон, углы, соотношения площади) и количество степеней свободы (количество параметров которые не фиксированы и могут меняться при трансформации).

Рассмотрим различные типы трансформаций для простоты в пространстве 2D.

Эвклидова трансформация сохраняет размеры и площадь. У нее 3 степени свободы.

Трансформация подобия имеет 4 степени свободы и сохраняет соотношение длин сторон и углы между ними.

Аффинная трансформация имеет 6 степеней свободы и сохраняет параллелизм сторон и соотношение площадей.

Перспективная трансформация имеет 8 степеней свободы и сохраняет параллелизм только для некоторых сторон.

Представление ориентации твердого тела

Классическим представлением ориентации твердого тела является матрица ориентации (точнее три матрицы). Также называются матрицы поворота. Формула поворота вокруг каждой конкретной оси (x, y и z):

Углы Эйлера

Вы когда-либо слышали об углах Эйлера? Углы Эйлера предоставляют простой способ представления ориентации в виде трёхмерного вектора. В аэронавтике и робототехнике обычно используются три параметра: Yaw, Pitch и Roll ([y, p, r]).

Недостатком углов Эйлера является тот факт что если угол Pitch равен 90 или -90 градусов, то мы теряем одну степень свободы (две оси совпадают). Это создаёт сингулярность что добавляет сложность к нашей проблеме. Эта проблема часто называется Gimbal lock.

На рисунке видно что все оси x, y, z совпали (предельный случай Gimbal lock).

Кватернионы

Кватернионы решают эту проблему. Кватернионы добавляют четвертую компоненту которая работает как мнимая часть комплексного числа.

Этот кватернион можно так же представить в следующем виде:

Рассмотрим некоторые операции над кватернионами.

Сложение и вычитание:

Умножение:

Норма:

Группы Ли и алгебра Ли

Эта тема довольно сложная и в материалах по ней обычно представлено очень много сложных математических определений и уравнений. Я решил дать определение которое мне выдало ИИ. Далее я привожу полностью оригинальный ответ от Gemini.

Алгебра Ли — это математический объект, похожий на векторное пространство, но с дополнительной операцией, называемой скобкой Ли или коммутатором, которая показывает, как объекты "переставляются". Группа Ли — это группа, которая также является гладким многообразием (непрерывным пространством). Группы Ли описывают непрерывные симметрии, а их алгебры Ли — бесконечно малые преобразования этих симметрий

Представьте себе вращение или перемещение объекта в пространстве — это непрерывные действия, которые можно выполнять много раз, и их можно выполнить в любом порядке (например, сначала повернуть на 90 градусов, потом на 180, или наоборот). Группа Ли — это множество всех таких возможных непрерывных преобразований, которое обладает свойствами группы (например, есть "ничего не делать", можно "развернуть" любое действие, и результат всегда однозначен).

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

Группы Ли и алгебра Ли облегчают вычисления трансформаций в непрерывном 3D пространстве (н-р, определение позиции камеры и элементов карты). Алгебра Ли предоставляет простое векторное представление, которое облегчает оценку состояния (state estimation), дифференцирование и оптимизацию. Таким образом группы и алгебра Ли эффективно решают сложные нелинейные проблемы в SLAM.

На хабре есть хорошая статья ^[3]на тему групп Ли и алгебры Ли. На Youtube есть хорошее видео ^[4]на эту тему.

Обзор библиотек для работы с трансформациями

Eigen

Для работы с линейной алгеброй используется библиотека Eigen. В Eigen реализованы специальные типы данных для хранения различных сущностей типа вектора, матрицы и тд. Библиотека облегчает работу с трансформациями, предоставляя удобные методы работы с ними.

Пример кода:

int main(int argc, char **argv) 
{
    Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
    q1.normalize();
    q2.normalize();
    
    Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
    Vector3d p1(0.5, 0, 0.2); // Create homogeneous transformation matrix from rotation and translation
    Isometry3d T1w(q1), T2w(q2);
    T1w.pretranslate(t1);
    T2w.pretranslate(t2); // Apply the transformation R1 to World, World to R2
    
    Vector3d p2 = T2w * T1w.inverse() * p1;
    cout << "p_R2 = T_R2_W * T_W_R1 * P_R1 = " << p2.transpose() << endl;
    return 0;
}

Sophus

Для работы с операцией из теории Lie используется библиотека Sophus ^[5].

Вот например один из примеров использования библиотеки Sophus - SO(3) и so(3) из репозитория ^[6] - трансформация между группой и алгеброй Ли и внесение маленького возмущения в матрицу поворота:

// Rotation of 90 deg around Z
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
Quaterniond q(R);

// ************  SO(3) ************
Sophus::SO3d SO3d_R(R); 
Sophus::SO3d SO3d_q(q); // Result should be the same as SO3d_R

cout << "SO(3) from rotation matrix = n" << SO3d_R.matrix() << endl;
cout << "SO(3) from quaternion = n" << SO3d_q.matrix() << endl;

// Logarithmic map to get the lie algebra
Vector3d so3 = SO3d_R.log();
cout << "so3 = n" << so3.transpose() << endl;

// Hat is from vector to skew-symmetric matrix
cout << "so3 hat = n" << Sophus::SO3d::hat(so3) << endl;

// Vee is from skew-symmetric matrix to vector
cout << "so3 vee = n" << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

// Update by perturbation model
Vector3d update_so3(1e-4, 0, 0);
Sophus::SO3d SO3d_updated = Sophus::SO3d::exp(update_so3) * SO3d_R;
cout << "SO3 updated = n" << SO3d_updated.matrix() << endl;
cout << "****************************" << endl;

Подключение Sophus в CMakeLists.txt

# Sophus practicefind_package( Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )
add_executable( useSophus useSophus.cpp)
target_link_libraries(useSophus Sophus::Sophus)

Визуализация траектории с Pangolin

Pangolin это набор легковесных и портируемых библиотек, который часто используется в сфере Computer Vision для визуализации работы визуального SLAM.

Для примера визуализируем траекторию из SLAM с помощью библиотеки Pangolin.

Для установки библиотеки Pangolin нужно последовать инструкции на гитхабе ^[7].

Скачайте файл траектории отсюда ^[8]. Исходные файлы примера можно найти здесь ^[9].

Создадим файл CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( geomtetry )
set(CMAKE_CXX_FLAGS "-std=c++14")
include_directories( "usr/include/eigen3" )

add_executable( eigenMatrix eigenMatrix.cpp )
add_executable( useGeometry useGeometry.cpp )
add_executable( coordinateTransform coordinateTransform.cpp )

# Pangolinfind_package(Pangolin REQUIRED)
find_package(pybind11 REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable( plotTrajectory plotTrajectory.cpp)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES} pybind11::embed)

Создадим файл plotTrajectory.cpp

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

using namespace Eigen;
using namespace std;

// path to trajectory file
string trajectory_file = "./examples/trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(trajectory_file);
    if (!fin) {
        cout << "cannot find trajectory file at " << trajectory_file << endl;
        return 1;
    }

    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
        Twr.pretranslate(Vector3d(tx, ty, tz));
        poses.push_back(Twr);
    }
    cout << "read total" << poses.size() << "pose entries" << endl;

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glLineWidth(2);
        for (size_t i = 0; i < poses.size(); i++) {
            // 画每个位姿的三个坐标轴
            Vector3d Ow = poses[i].translation();
            Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
            Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
            Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
            glBegin(GL_LINES);
            glColor3f(1.0, 0.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Xw[0], Xw[1], Xw[2]);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Yw[0], Yw[1], Yw[2]);
            glColor3f(0.0, 0.0, 1.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Zw[0], Zw[1], Zw[2]);
            glEnd();
        }
        // 画出连线
        for (size_t i = 0; i < poses.size(); i++) {
            glColor3f(0.0, 0.0, 0.0);
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i+1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000); // sleep 5 ms
    }
}

Скомпилируем и запустим приложение

На этом пока все. Мы покрыли одну из самых сложных глав теории о визуальном SLAM. Еще раз напомню, что есть хорошая книга SLAM Handbook где очень подробно разбирается теория SLAM (с большим количеством математики). Скачать ее можно здесь ^[10].

Жду комментариев и вашего мнения насколько материал оказался для вас полезен чтобы мне планировать последующие статьи из цикла. Всем удачи!