In this paper, we will implement the feature point extraction in LIo-SAM under two-dimensional lidar.
The purpose is to get familiar with the processing mode of lidar data, experience how to process lidar data and how to extract feature points.
Lio-sam’s project address is: github.com/TixiaoShan/…
The feature point extraction of LIo-SAM is basically the same as that of LOAM, but the specific calculation method of curvature value is slightly different.
1. Effect of feature point extraction
Let’s first take a look at the original laser data, as shown in the figure below:
Next, take a look at the appearance of the data points after we extract the feature points, as shown in the figure below:
As you can see, the data points after feature point extraction are not very intuitive, but they are distributed fairly evenly.
Next, let’s take a look at how to extract feature points from a code perspective.
2 Code download
I’m not going to go through the code step by step starting with this section. I’m going to cover only the important parts of the code in this article. The complete project is available for download on Github.
Github address, please reply to open source address in my official number: From scratch to Laser SLAM.
Git Clone is recommended, because the code is being updated, git Clone downloaded code can be easily updated using Git pull.
3 Operating Environment
3.1 Project Directory
Please place the downloaded code folder Creating-2D-laser-slam-from-scratch under ~/catkin_ws/ SRC /.
3.2 Directory of the bag file
And put the bag data in ~/bagfiles/ folder.
For the data package corresponding to this article, please reply to Lesson1 on my official account.
3.3 Operating Environment
Follow the environment Settings in the previous article.
3.3 Code running
Enter this command in the terminal to run the launch I’ve already written in the project and configured the rVIz display module
roslaunch lesson1 feature_detection.launch
Copy the code3.4 Launch file explanation
The location of the bag file needs to be configured according to the user name of your computer
First, set the parameter use_sim_time to true, which means the current ROS time is the time in the bag file.
Also, you need to add the –clock option when playing the bag to indicate the current time when the time in the bag file is used as ros.
At the same time, Rviz was started and a configured RVIz profile was loaded.
<launch>
<! -- address and name of bag -->
<arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>
<! -- use the time stamp of bag -->
<param name="use_sim_time" value="true" />
<! -- Start node -->
<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>
Copy the code4 Code Explanation
The comments in the code are largely clear.
4.1 the main
int main(int argc, char **argv)
{
ros::init(argc, argv, "lesson1_feature_detection_node"); // Name of the node
LaserScan laser_scan;
ros::spin(a);// At this point, the program starts to wait and executes ScanCallback() each time a subscribed message arrives.
return 0;
}
Copy the code4.2 Data Types
Smoothness_t defines a key-value pair data type for itself
By_value is the sorting method used by the sorting function sort, which means sorting from small to large, depending on the size of the value in smoothness_t.
#define max_scan_count 1500 // The maximum number of radar data
struct smoothness_t
{
float value;
size_t index;
};
// Sort from smallest to largest
struct by_value
{
bool operator(a)(smoothness_t const &left, smoothness_t const &right)
{
returnleft.value < right.value; }};Copy the code4.3 engineering class
// Declare a class
class LaserScan
{
private:
ros::NodeHandle node_handle_; // Handle in ros
ros::NodeHandle private_node_; // Private handle in ros
ros::Subscriber laser_scan_subscriber_; // Declare a Subscriber
ros::Publisher feature_scan_publisher_; // Declare a Publisher
float edge_threshold_; // Extract the threshold of the corner
public:
LaserScan(a); ~LaserScan(a);void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
Copy the code4.4 Constructors
The callback function for subscribing to Laser +scan is set to: ScanCallback()
I initialized a Publisher and published the extracted feature points (sensor_MSgs ::LaserScan) to the topic Feature_scan.
// The constructor
LaserScan::LaserScan() : private_node_("~")
{
// The \033[1;32m, \033[0m terminal is green
ROS_INFO_STREAM("\033[1;32m----> Feature Extraction Started.\033[0m");
// Bind the radar callback function to the subscribed topic
laser_scan_subscriber_ = node_handle_.subscribe("laser_scan".1, &LaserScan::ScanCallback, this);
// Publish the extracted points to the topic feature_scan
feature_scan_publisher_ = node_handle_.advertise<sensor_msgs::LaserScan>("feature_scan".1.this);
// Set the threshold for corner extraction to 1.0
edge_threshold_ = 1.0;
}
Copy the code4.5 Callback functions
Here’s the code for the callback function:
void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
Copy the code4.5.1 Temporary variable declaration
First, some temporary variables are declared and initialized at the beginning of the code for the callback function.
(The habit of declaring too many temporary variables is actually a bad one. Every time you declare a new variable, you have to call the constructor, which is a waste of time, but I put it here for clarity.)
std::vector<smoothness_t> scan_smoothness_(max_scan_count); // Store the curvature and index of each point
float *scan_curvature_ = new float[max_scan_count]; // Store the curvature of each point
std::map<int.int> map_index; // The valid point index corresponds to the actual scan index
int count = 0; // Index of valid points
float new_scan[max_scan_count]; // Store the distance value of scan data
// Traverse the radar data by the number of data in ranges
int scan_count = scan_msg->ranges.size(a);Copy the code4.5.2 Remove invalid points
What this for loop implements is to remove the invalid point.
Because there are a lot of INF or NAN values in radar data that will cause errors in math, remove them before using range data.
If you don’t know these two values, you can check them. There are many reasons for this data, such as the Angle between the wall and the radar ray is too large, a line of radar hits the edge of the table, resulting in a very low return intensity and so on. In these cases, the radar driver package will assign the value to either INF or NAN.
STD ::isfinite will return false if an invalid value is entered, so reverse.
Place the distance value of the valid point in new_SCAN for later use.
We use count as the index of new_scan, and count is continuous.
// Remove the inf or NAN point and save the valid point
for (int i = 0; i < scan_count; i++)
{
// STD ::isfinite: returns true if the input value is valid
if(! std::isfinite(scan_msg->ranges[i]))
{
// std::cout << " " << i << " " << scan_msg->ranges[i];
continue;
}
// The index of this point in the original data is I, and the index of this point in new_scan is count
map_index[count] = i;
// New_scan holds the distance value of the valid point
new_scan[count] = scan_msg->ranges[i];
count++;
}
Copy the code4.5.3 Calculate the curvature value
Calculate the curvature value of each point in new_scan. The curvature here is not calculated strictly according to the curvature calculation formula. Instead, the distance between the 5 points before and after the current point and the current point is calculated by a difference of 10 times.
Is the average deviation between the current point and five points before and after it, as the value of curvature.
The squares of curvature values are stored in scan_curvature_ and scan_smoothness_ for backup.
// Calculate the value of curvature, which is represented by the deviation of the distance values of five points before and after the current point
// If it is a sphere, then the sum of the distances of the 10 points around the current point minus 10 times the distance of the current point should be equal to 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;
}
Copy the codeNote that the two for loops above can actually be combined into one to reduce the running time
Declare a queue for the first 11 valid values and enter the queue. For the last 11 valid values, perform the operation out of the team; For the values during this period, first calculate the curvature of the 11 values, then eject the first value at the head of the queue, and insert a new value at the end of the queue to complete the operation of the second for loop, so as to reduce the amount of computation.
4.5.4 Declare a sensor_MSgs ::LaserScan
Declare a temporary sensor_MSgs ::LaserScan variable to store scan feature points after feature extraction and publish them for display in RVIz.
// Declare a temporary sensor_msgs::LaserScan variable to store scan data after feature extraction and publish it for display in 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;
// Initialize float[]
corner_scan.ranges.resize(max_scan_count);
Copy the code4.5.5 Feature point extraction
First, in order to ensure a more uniform distribution of feature points, scan data is divided into six parts, and each part takes a maximum of 20 feature points.
First, calculate the index start_index and end_index of the starting and ending points of the data segment, and then sort the data segment from small to large according to the curvature value. Sort ().
So that the points with the larger curvature are in the latter part, we iterate backwards to pick up to 20 points and populate the corner_scan of the new declaration with those points.
// Perform corner extraction, divide the complete SCAN into 6 parts, and extract 20 corners in each part
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;
// Order the point clouds in order of curvature from smallest to largest
std::sort(scan_smoothness_.begin() + start_index,
scan_smoothness_.begin() + end_index, by_value());
int largestPickedNum = 0;
// The final point has the maximum curvature, which is the corner point if the conditions are met
for (int k = end_index; k >= start_index; k--)
{
int index = scan_smoothness_[k].index;
if (scan_smoothness_[k].value > edge_threshold_)
{
// Select at most 20 corners for each segment
largestPickedNum++;
if (largestPickedNum <= 20)
{
corner_scan.ranges[map_index[index]] = scan_msg->ranges[map_index[index]];
}
else
{
break; }}}}Copy the code4.5.6 Release of data
// Publish the extracted SCAN data
feature_scan_publisher_.publish(corner_scan);
Copy the code5 concludes
Drawing on the feature extraction in LIO-SAM, we realized a simple feature point extraction function using single-line radar, and experienced the process of processing lidar data.
There are some additional operations in LIo-SAM, such as marking occlusion points and parallel points. After extracting a feature point, the five data before and after this feature point will be marked and no longer participate in feature point extraction.
We show the extraction of corner feature, and the extraction of plane point feature in LIO-SAM. Since this part uses multi-line data points, this part is not implemented in single-line radar.
Single-line radar can also carry out the extraction of straight line, line segment and other features, here is no longer implemented.
More compared to SAM’s feature points extraction code reading, please see this article blog.csdn.net/tiancailx/a…
The next article is tentatively: using ICP in PCL, the realization of front-end odometer.
The article will be updated in the public number: from scratch with SLAM, welcome everyone to follow, in order to update the article in the first time to inform you.
At the same time, I also hope that you can recommend this official account to the people around you who do laser SLAM, so that we can make progress together.
If you have any suggestions on the article I wrote, or want to see which aspect of the function is implemented, please reply directly in the official account, I can receive, and will seriously consider your suggestions.
