lidar_camera_calib

[TOC]

1. Основная идея

Извлечение пиксельных координат калибровочного доски на изображении и извлечение 3D-координат углов калибровочного доски в соответствующем лазерном кадре. Решение задачи соответствия 2D-3D для определения матрицы Rt. Для извлечения углов на изображении используется libcbdetect, а для извлечения углов в лазерных данных используется метод ILCC. Этот метод требует высокого качества калибровочного доски, чтобы разница в отражательной способности белой и черной областей была заметной. В качестве примера я использую калибровку камеры PointGrey и лазерного сканера Velodyne 16-канального.

2. Сбор данных

Рекомендуется, чтобы расстояние между лазером и камерой было как можно меньше (это опыт, пока неизвестны теоретические основания).

Сенсоры должны быть неподвижными, а данные должны быть собраны в разных местах. Лучше, чтобы каждая линия лазера попадала на калибровочный доски. Ниже приведен пример записи bag-файла, параметр -l NUM указывает на то, что нужно записать только NUM сообщений на каждом топике.

rosbag record /camera_topic /lidar_topic -l 3

Обычно данные собираются с 6 до 8 различных углов. Ниже приведены примеры сбора данных. Записанные bag-файлы сохраняются в папке bag. ### 3. Калибровка камеры

Калибровка камеры выполняется с помощью инструментария для калибровки камер в MATLAB, результаты калибровки сохраняются в следующем yaml-формате. Здесь также заданы три параметра калибровочного доски: количество углов вдоль осей X и Y, а также размер одной ячейки (в метрах).

%YAML:1.0

K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [1061.37439737547, 0, 980.706836288949, 0, 1061.02435228316, 601.685030610243, 0, 0, 1]
d: !!opencv-matrix
   rows: 5
   cols: 1
   dt: d
   data: [-0.149007007770170, 0.0729485326193990, 0.000257753168848673, -0.000207183134328829, 0]

Camera.width: 1920
Camera.height: 1200

grid_length: 0.15
corner_in_x: 7
corner_in_y: 5

Результаты сохраняются в директории config с именем pointgrey.yaml.

Запуск image_corners.launch, получаем изображения без искажений, которые сохраняются в директории process_data.

Не забудьте изменить параметры в launch-файле. bag_path_prefix должен содержать путь к bag-файлу и префикс имени bag-файла.

roslaunch ilcc2 image_corners.launch

4. Извлечение углов шахматной доски с помощью libcbdetect

Метод извлечения шахматной доски с использованием OpenCV и camodocal требует определенных условий для изображений, в то время как LIBCBDETECT практически всегда успешно извлекает углы шахматной доски. Поэтому используется версия для MATLAB libcbdetect, а версия для C++ доступна здесь.В данном разделе представлены модифицированные версии libcbdetect для MATLAB. С помощью demo.m и demo_all_pic.m можно извлечь углы шахматной доски. Ниже приведен пример извлечения углов, где первая половина представляет собой горизонтальные координаты, а вторая — вертикальные (и наоборот).

1143.7 1143 1142.5 1142 1141.4
1060.6 1060.2 1059.9 1059.7 1059.5
979.01 978.81 978.78 978.83 978.83
898.94 899.05 899.23 899.47 899.78
820.18 820.41 820.9 821.33 821.84
742.66 743.33 743.9 744.68 745.51
666.68 667.48 668.4 669.35 670.46
398.18 481.36 564.03 646.08 727.64
399.5 481.91 563.87 645.24 726.04
400.79 482.44 563.67 644.36 724.52
402.03 482.98 563.47 643.46 722.98
403.23 483.52 563.33 642.6 721.49
404.51 484.15 563.2 641.77 719.95
405.82 484.64 563.03 640.99 718.46

5. Извлечение углов шахматной доски с помощью ilcc2

См. ILCC. Здесь реализована версия для C++.

Обратите внимание на параметры в launch файле. Необходимо изменить путь к bag файлу.

roslaunch ilcc2 lidar_corners.launch

Программа откроет два окна pcl_viewer: visual_corners и visual_chessboard. Обратите внимание на вывод в терминале! В терминале будет указано, что необходимо опубликовать топик /click_point. Программа будет использовать этот топик для определения области интереса (ROI) и извлечения плоскостей в этой области. Откройте rviz и отобразите данные лазера. Включите функцию Publish Point в rviz, выберите точку в центре областей шахматной доски. Затем в окне visual_chessboard будет отображена область извлечения, которая будет показана розовыми точками.

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

o: confirm;  r: change /click_point; w: plane_idx++; s: plane_idx--; now plane_inx = 1

При получении результата, показанного на правом рисунке, попробуйте нажать w. **Обратите внимание, что нажатие клавиш должно происходить в активном окне visual_chessboard, а не в терминале!**Еще раз обратите внимание, если нажатие клавиш не вызывает реакции, это может быть связано с тем, что нажатие происходит не в окне visual_chessboard.

Если извлечение углов выполнено правильно, нажатие клавиши o приведет к отображению результатов извлечения углов в интерфейсе visual_corners. Всего два изображения шахматной доски, одно из которых соответствует лазерному источнику, а другое — исходному положению. На изображении в исходном положении есть розовые точки, которые являются лазерными углами. Обратите внимание на лазерные углы и действуйте в соответствии с подсказками в терминале.

k: confirm;  d: do optim again; a: rotate board; r:reject this scan

При получении результата, показанного на левом рисунке, попробуйте нажать d; при получении результата, показанного на правом рисунке, попробуйте нажать a. Конечный правильный результат представлен на следующем рисунке. Розовые углы извлечены правильно. Нажмите k для подтверждения. Обратите внимание, что нажатие клавиш должно происходить в активном окне visual_corners. Затем для данных с других направлений применяются аналогичные шаги. Итог:

• В rviz используйте функцию Publish Point для публикации топика /click_point.
• Обратите внимание на розовые облака точек в интерфейсе visual_chessboard и убедитесь, что они соответствуют точкам шахматной доски. Нажмите o для подтверждения.
• В интерфейсе visual_corners проверьте, правильно ли извлечены розовые углы. Нажмите k для подтверждения.

Результаты извлечения углов лазера сохраняются в папке process_data. Пример извлечения углов представлен ниже. Каждая строка представляет положение одного из 35 углов шахматной доски в системе координат лазера.

2.08881 0.609289 0.302605
2.07478 0.459948 0.30224
2.06074 0.310606 0.301874
......
2.06503 0.0103461 -0.29798
2.05099 -0.138995 -0.298345
2.03696 -0.288337 -0.29871

6. Калибровка внешних параметров лазера и камеры

Теперь в папке process_data есть файлы углов лазера, углов камеры и изображения без искажений.

На основе этих данных можно калибровать внешние параметры лазера и камеры.

roslaunch ilcc2 calib_lidar_cam.launch

Перед запуском необходимо изменить функцию установки начальных внешних параметров лазера и камеры в файле ilcc2/test/calib_lidar_cam.cpp.

Если лазер и камера установлены, как показано на следующем схематическом рисунке, аналогично случаю cam_name == "front".Камера повернута на 90 градусов влево, когда cam_name == "left". Ситуация калибровки PointGrey и Velodyne аналогична случаю, когда cam_name == "front". Можно установить начальные значения внешних параметров сенсоров в зависимости от способа их установки, чтобы обеспечить согласованность направлений координатных систем у минимум двух сенсоров!```cpp /// Это для согласования направлений координатных систем камеры и лазера Eigen::Isometry3d get_lidar2cam_axis_roughly(std::string cam_name){

Eigen::AngleAxisd R_lidarToCamera; if(cam_name == "front" || cam_name == "car_left" || cam_name == "pointgrey") R_lidarToCamera = Eigen::AngleAxisd(-1.57, Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitX()); if(cam_name == "left") R_lidarToCamera = Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitX()); if(cam_name == "right") R_lidarToCamera = Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(3.14, Eigen::Vector3d::UnitZ()); if(cam_name == "back") R_lidarToCamera = Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitX());

Eigen::Isometry3d T_lidar2cam_axis_roughly = Eigen::Isometry3d::Identity(); T_lidar2cam_axis_roughly.rotate(R_lidarToCamera.matrix());

return T_lidar2cam_axis_roughly; }

<img src="./pic/lidar_camera_corners1.png" width="40%"/> <img src="./pic/lidar_camera_corners2.png" width="40%"/>

Если начальные внешние параметры установлены правильно, в окне `lidar_camera_corners` можно увидеть зелёные угловые точки камеры и красные угловые точки лазера. Это делается для обеспечения соответствия угловых точек лазера и камеры. Достаточно, чтобы красные и зелёные точки были расположены в одинаковом порядке (обычно это не вызывает проблем, можно пропустить нажатием любой клавиши).

Если красные точки лазера не видны, возможно, начальные внешние параметры установлены неверно, и потребуется пересмотр функции `get_lidar2cam_axis_roughly`.



<img src="./pic/calib_result1.png" width="40%"/><img src="./pic/calib_result2.png" width="40%"/>После калибровки будет отображён результат переотображения (как показано на приведённых выше изображениях). Конечные результаты калибровки будут выведены в терминал и сохранены в виде bin-файлов в папке config.

### 7. Эффект проекции лазера на изображение камеры

Обратите внимание на параметры в launch.

roslaunch ilcc2 pcd2image.launch

Необходимо самостоятельно публиковать топики лазера и камеры. Если требуется использовать калиброванные данные bag, можно использовать параметр `-l`, чтобы выполнить циклическое воспроизведение.```
rosbag play bagname -l

8. Проекция камеры на лазер

Обратите внимание на параметры в launch.

roslaunch ilcc2 rgb_lidar.launch

Необходимо самостоятельно публиковать топики лазера и камеры. Поскольку pointgrey — это монохромная камера, она не может предоставить цветные точки облака.

9. Другие изображения