Transformations

Visual Odometry

Common odometry stuff for rgbd_odometry and stereo_odometry nodes. These nodes wrap the odometry approach of RTAB-Map. The approach is visual based so enough visual features must be present in images to compute the odometry. When a transformation cannot be computed, a null transformation is sent to notify the receiver that odometry is not updated or lost.

Required tf Transforms
base_link → usually a fixed value, broadcast periodically by a robot_state_publisher, or a tf static_transform_publisher.

Provided tf Transforms
odom → base_link the current estimate of the robot's pose within the odometry frame.

Этот набор Node предоставляет одометрию на основе стереокамер, rgbd камер и т.д. В главном launch файле rtabmap.launch можно увидеть, как используется одна из набора этих нод. Мы очень часто пользовались другим launch файлом rgbd_mapping.launch, который в свою очередь вызывает указанный выше.

Идея решения

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

В данный момент я нашел 2 способа сделать это:

Const смещение base_link -> camera_link

Описание

1. Добавить const смещение между odom и камерой. Чтобы облако строилось не в той системе координат, в которой находится камера, а смещенной с.к. (новое смещение base_link -> camera_link приведет к тому, что облако точек будет строиться в с.к. смещенной камеры).

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

Здесь очень хорошая документация о системах координат в ros: Coordinate Frames for Mobile Platforms

Реализация

Для этого необходимо добавить смещение base_link > camera_link Далее rtabmap, изпользуя одну из нод visual odometry должна начать выдавать odom > base_link вместо odom > camera_link Для этого вероятно необходимо поработать над этим rgbd_mapping.launch файлом, т.к. он почему - то не дает образоваться этой связи, хоть и запускает visual odometry. Вероятно это сделано потому, что это пример для hand-held kinect mapping, где не предпологалось существования мобильной платформы.

rgbd_odometry

Удалось запустить Node rgbd_odometry отдельно. В этом случае, результат соответствовал ожиданиям (visual odometry выдавал трансформацию odom > base_link)

Скрипты можно найти в репозитории в папке Transformations
Результаты эксперимента можно увидеть здесь: camera_link > base_link

rgbd_mapping

Удалось запустить rtabmap SLAM с исправленной версией rgbd_mapping.launch файла. Для этого потребовалось:

• Запустить static_tf_publisher (base_link > camera_link)
• Запустить Visual Odometry, которая работала с base_link.
• Запустить SLAM с frame_id (с.к.) камеры (camera_link) См. launch файл репозитория rgbd_mapping_with_offset.launch

Нужны тесты

~initial_pose

2. У семейства Node visual odometry есть параметр (initialpose)

~initial_pose (string, default: "") The initial pose of the odometry. Format: "x y z roll pitch yaw".

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

Скрипт для тестирования также имеется в репозитории: rgbd_mapping_with_initial_pose.launch