В этой статье мы реализуем часть LIO-SAM для извлечения характерных точек под 2D-лидар.
Целью реализации является знакомство с методом обработки лидарных данных, опыт обработки лидарных данных и извлечения характерных точек.
Адрес проекта LIO-SAM:GitHub.com/TI СяоШань/…
Часть выделения характерных точек в LIO-SAM в основном такая же, как и в LOAM, но конкретный метод расчета при расчете значения кривизны немного отличается.
1 Эффект после извлечения характерных точек
Давайте сначала посмотрим, как выглядят необработанные лазерные данные, как показано ниже:
Далее, давайте посмотрим на точки данных после того, как мы извлечем характерные точки, как показано на следующем рисунке:
Видно, что точки данных после извлечения характерных точек не очень интуитивны, но распределение достаточно равномерное.
Далее давайте посмотрим, как извлекать характерные точки с точки зрения кода.
2 Загрузка кода
Начиная с этого раздела, я не буду писать код пошагово, в статье я только объясню смысл важной части кода, пожалуйста, скачайте полный проект на github.
Адрес github, пожалуйста, обратитесь к моей публичной учетной записи:Создайте лазерный SLAM с нуляОтветить вадрес с открытым исходным кодомполучить.
Рекомендуемое использованиеgit cloneспособ загрузки, поскольку код обновляется, можно использовать код, загруженный клоном gitgit pullЭто легко обновить.
3 Операционная среда
3.1 Каталог проекта
Загрузите папку с кодомCreating-2D-laser-slam-from-scratchставить~/catkin_ws/src/под папку.
3.2 Каталог файлов сумок
и введите данные сумки~/bagfiles/под папку.
Пакет данных, соответствующий этой статье, пожалуйста, ответьте в моем официальном аккаунте.lesson1получить.
3.3 Операционная среда
Пожалуйста, следуйте настройкам среды в предыдущей статье, чтобы настроить его.
3.3 Запуск кода
Введите эту строку команды в терминал для запуска, запуска, который я написал в проекте, и настройте модуль отображения rviz.
roslaunch lesson1 feature_detection.launch
3.4 Пояснение к файлу запуска
Расположение файла пакета должно быть настроено в соответствии с именем пользователя вашего компьютера.
Во-первых, для параметризацииuse_sim_timeУстановите значение true, что означает, что текущее время ros соответствует времени в файле сумки.
В то же время вам нужно добавить его при игре в мешок--clockВозможность использовать время в файле сумки в качестве текущего времени ros.
В то же время запускается rviz и загружается настроенный файл конфигурации rviz.
<launch>
<!-- bag的地址与名称 -->
<arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>
<!-- 使用bag的时间戳 -->
<param name="use_sim_time" value="true" />
<!-- 启动节点 -->
<node name="lesson1_laser_scan_node" pkg="lesson1" type="lesson1_feature_detection_node" output="screen" />
<!-- launch rviz -->
<node name="rviz" pkg="rviz" type="rviz" required="false"
args="-d $(find lesson1)/launch/feature.rviz" />
<!-- play bagfile -->
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
4 Объяснение кода
Комментарии в коде в основном объясняются.
4.1 main
int main(int argc, char **argv)
{
ros::init(argc, argv, "lesson1_feature_detection_node"); // 节点的名字
LaserScan laser_scan;
ros::spin(); // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
return 0;
}
4.2 Типы данных
smoothness_t определяет для себя тип данных пары ключ-значение
by_value — это метод сортировки, используемый функцией сортировки sort, что означает сортировку от меньшего к большему в зависимости от размера значения в smoothness_t для сортировки от меньшего к большему.
#define max_scan_count 1500 // 雷达数据个数的最大值
struct smoothness_t
{
float value;
size_t index;
};
// 排序的规则,从小到大进行排序
struct by_value
{
bool operator()(smoothness_t const &left, smoothness_t const &right)
{
return left.value < right.value;
}
};
4.3 Класс лазерного сканирования
// 声明一个类
class LaserScan
{
private:
ros::NodeHandle node_handle_; // ros中的句柄
ros::NodeHandle private_node_; // ros中的私有句柄
ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber
ros::Publisher feature_scan_publisher_; // 声明一个Publisher
float edge_threshold_; // 提取角点的阈值
public:
LaserScan();
~LaserScan();
void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
4.4 Конструкторы
Функция обратного вызова, которая подписывается на лазер+сканирование, задается в конструкторе: ScanCallback()
Инициализируйте издателя, опубликуйте извлеченные характерные точки, тип данных — sensor_msgs::LaserScan, и опубликуйте в теме feature_scan.
// 构造函数
LaserScan::LaserScan() : private_node_("~")
{
// \033[1;32m，\033[0m 终端显示成绿色
ROS_INFO_STREAM("\033[1;32m----> Feature Extraction Started.\033[0m");
// 将雷达的回调函数与订阅的topic进行绑定
laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &LaserScan::ScanCallback, this);
// 将提取后的点发布到 feature_scan 这个topic
feature_scan_publisher_ = node_handle_.advertise<sensor_msgs::LaserScan>("feature_scan", 1, this);
// 将提取角点的阈值设置为1.0
edge_threshold_ = 1.0;
}
4.5 Функции обратного вызова
Затем объясните код в функции обратного вызова:
void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
4.5.1 Объявление временной переменной
Во-первых, в начале кода callback-функции объявляются и инициализируются некоторые временные переменные.
(Привычка объявлять слишком много временных переменных на самом деле нехороша. Каждый раз, когда объявляется новая переменная, будет время на вызов конструктора, что является пустой тратой времени, но его временно помещают сюда ради кода ясность.)
std::vector<smoothness_t> scan_smoothness_(max_scan_count); // 存储每个点的曲率与索引
float *scan_curvature_ = new float[max_scan_count]; // 存储每个点的曲率
std::map<int, int> map_index; // 有效点的索引 对应的 scan实际的索引
int count = 0; // 有效点的索引
float new_scan[max_scan_count]; // 存储scan数据的距离值
// 通过ranges中数据的个数进行雷达数据的遍历
int scan_count = scan_msg->ranges.size();
4.5.2 Удалить неверные точки
Функция, реализованная этим циклом for, заключается в удалении недопустимой точки.
Потому что будет многоинф или нанЗначение этого значения будет сообщать об ошибке при выполнении математических операций, поэтому это значение следует удалить перед использованием данных диапазона.
Если вы не знаете эти два значения, вы можете использовать Baidu.Причин для такого рода данных много.Например, угол между стеной и лучом радара слишком велик, и линия радара попадает на край стол, что приводит к очень низкой интенсивности возврата и т. д. . В этих случаях пакет драйвера радара присвоит этому значению значение inf или nan.
std::isfinite вернет false при вводе недопустимого значения, поэтому отмените его.
Поместите значение расстояния действительной точки в new_scan для последующего использования.
Мы используем count как индекс new_scan, count непрерывен.
// 去处inf或者nan点,保存有效点
for (int i = 0; i < scan_count; i++)
{
// std::isfinite ：输入的值是有效值，返回true
if (!std::isfinite(scan_msg->ranges[i]))
{
// std::cout << " " << i << " " << scan_msg->ranges[i];
continue;
}
// 这点在原始数据中的索引为i，在new_scan中的索引为count
map_index[count] = i;
// new_scan中保存了有效点的距离值
new_scan[count] = scan_msg->ranges[i];
count++;
}
4.5.3 Расчет значения кривизны
Рассчитайте значение кривизны каждой точки в new_scan, Упомянутая здесь кривизна не рассчитывается строго по формуле расчета кривизны, а рассчитывается по сумме значений расстояния пяти точек до и после текущей точки и значение расстояния текущей точки 10-кратная разница, используйте эту разницу для аппроксимации значения кривизны кода.
Это средняя степень отклонения между текущей точкой и пятью точками до и после нее, как значение кривизны.
И сохраните квадрат значения кривизны в scan_curvature_ и scan_smoothness_ для использования.
// 计算曲率值, 通过当前点前后5个点距离值的偏差程度来代表曲率
// 如果是球面, 则当前点周围的10个点的距离之和 减去 当前点距离的10倍 应该等于0
for (int i = 5; i < count - 5; i++)
{
float diff_range = new_scan[i - 5] + new_scan[i - 4] +
new_scan[i - 3] + new_scan[i - 2] +
new_scan[i - 1] - new_scan[i] * 10 +
new_scan[i + 1] + new_scan[i + 2] +
new_scan[i + 3] + new_scan[i + 4] +
new_scan[i + 5];
// diffX * diffX + diffY * diffY
scan_curvature_[i] = diff_range * diff_range;
scan_smoothness_[i].value = scan_curvature_[i];
scan_smoothness_[i].index = i;
}
Уведомлениедва приведенных выше цикла for можно объединить в один, чтобы сократить время выполнения.
объявить очередь Поставьте в очередь первые 11 допустимых значений; Удалить из очереди последние 11 допустимых значений; Для значений в течение этого периода сначала вычислите кривизну этих 11 значений, затем поместите первое значение в начало очереди, а затем вставьте новое значение в конец очереди, чтобы завершить операцию второго для петля для достижения операции сокращения цель количества.
4.5.4 Объявление sensor_msgs::LaserScan
Объявите временную переменную sensor_msgs::LaserScan для хранения характерных точек сканирования после извлечения признаков и публикации их для отображения в rviz.
// 声明一个临时的sensor_msgs::LaserScan变量,用于存储特征提取后的scan数据,并发布出去,在rviz中进行显示
sensor_msgs::LaserScan corner_scan;
corner_scan.header = scan_msg->header;
corner_scan.angle_min = scan_msg->angle_min;
corner_scan.angle_max = scan_msg->angle_max;
corner_scan.angle_increment = scan_msg->angle_increment;
corner_scan.range_min = scan_msg->range_min;
corner_scan.range_max = scan_msg->range_max;
// 对float[] 进行初始化
corner_scan.ranges.resize(max_scan_count);
4.5.5 Извлечение характерных точек
Во-первых, чтобы обеспечить более равномерное распределение характерных точек, данные сканирования разделены на 6 частей, и каждая часть занимает до 20 характерных точек.
Сначала вычислите индексы start_index и end_index начальной и конечной точек одной шестой сегмента данных, а затем отсортируйте эти данные по значению кривизны от малого к большому sort().
Таким образом, точки с большими значениями кривизны находятся в последней части.Мы выбираем до 20 точек путем обхода сзади вперед и заполняем эти точки вновь объявленным angle_scan.
// 进行角点的提取,将完整的scan分成6部分,每部分提取20个角点
for (int j = 0; j < 6; j++)
{
int start_index = (0 * (6 - j) + count * j) / 6;
int end_index = (0 * (5 - j) + count * (j + 1)) / 6 - 1;
// std::cout << "start_index: " << start_index << " end_index: " << end_index << std::endl;
if (start_index >= end_index)
continue;
// 将这段点云按照曲率从小到大进行排序
std::sort(scan_smoothness_.begin() + start_index,
scan_smoothness_.begin() + end_index, by_value());
int largestPickedNum = 0;
// 最后的点 的曲率最大，如果满足条件，就是角点
for (int k = end_index; k >= start_index; k--)
{
int index = scan_smoothness_[k].index;
if (scan_smoothness_[k].value > edge_threshold_)
{
// 每一段最多只取20个角点
largestPickedNum++;
if (largestPickedNum <= 20)
{
corner_scan.ranges[map_index[index]] = scan_msg->ranges[map_index[index]];
}
else
{
break;
}
}
}
}
4.5.6 Выпуск данных
// 将提取后的scan数据发布出去
feature_scan_publisher_.publish(corner_scan);
5 Резюме
Мы используем часть извлечения признаков в LIO-SAM, чтобы реализовать простую функцию извлечения точек признаков с использованием однолинейного радара, и познакомимся с процессом обработки лидарных данных.
В LIO-SAM есть некоторые дополнительные операции, такие как маркировка точек окклюзии, параллельных точек, и после извлечения каждой характерной точки пять данных до и после характерной точки помечаются и больше не участвуют в извлечении характерной точки и других операциях. .
Мы показываем извлечение функций угловых точек и извлечение функций плоских точек в LIO-SAM, Поскольку в этой части используются многострочные точки данных, эта часть не реализована в однолинейном радаре.
Однолинейный радар также может извлекать такие функции, как прямые линии и линейные сегменты, которые здесь реализованы не будут.
Дополнительные сведения о коде извлечения характерных точек LIO-SAM см. в этой статье.blog.CSDN.net/Genius Leader/Ах…
Предварительно запланирована следующая статья: использование ICP в PCL для реализации внешнего одометра.
Статья будет вОфициальный аккаунт: Создайте SLAM с нуляСинхронизированные обновления, каждый может обратить внимание, чтобы уведомить вас, как только статья будет обновлена.
В то же время я также надеюсь, что вы порекомендуете этот официальный аккаунт окружающим вас людям, которые занимаются лазерным SLAM, чтобы все вместе могли добиться прогресса.
Если у вас есть предложения по статьям, которые я написал, или вы хотите посмотреть, как реализованы функции, пожалуйста, ответьте прямо в официальном аккаунте, я могу их получить и серьезно рассмотрю ваши предложения.