The previous article only visualized the data radar in rviz, and did not actually use it.
This article will use Kaist Urban08 data to construct a 3D point cloud map using LIO-SAM.
1 clone project
The address of the warehouse is
https://github.com/xiangli0608/Learning_localization_from_scratch_ws
The code has been written, just compile it directly.
Since LIO_SAM is put in, it needs to depend on gtsam 4.0.2.
2 bags of transcription
I said before that the data can be released in the form of file_player, but it is not as convenient to play the data through the bag. So this step first converts the kaist dataset into a bag file.
The tool used for transcription is kaist2bag, which has been placed in the project and can be used directly in src/kaist_tool/kaist2bag.
This is someone else's open source project, and it is used directly here. First of all, I would like to thank this old man.
How to use it is described in kaist2bag/README.md, here is a brief explanation.
2.1 Modify the path
First change src/kaist_tool/kaist2bag/config/config.ymal
Change the first 2 lines, dataset is the path of the dataset, and save_to is the path to save the newly generated bag
dataset: "/home/touchair/kaist_urban_dataset/urban08"
save_to: "/home/touchair/kaist_urban_dataset/urban08/bag"
In this config, you can also choose whether to output certain topics.
2.2 generate bag
Then execute launch with the following commandroslaunch kaist2bag kaist2bag.launch
After execution, the program will run for a period of time. After the execution, many bags will be generated in the svae_to folder, one bag for each topic.
The next step is to merge these bags.
2.3 Merge bag
The bag generated in the previous step is one topic and one bag, so if you want to run it, you need to merge these bags.
The command to merge rosrun kaist2bag mergebag.py merged.bag <bag_file_1> ... <bag_file_8>
is merged.bag is the last output bag, <bag_file_1> and the name of the bag after that are the bags that need to be merged. You can use an absolute path or a relative path here.
Through this step, we converted the kaist dataset into a bag.
This part refers to the article
KAIST dataset converted to rosbag https://zhuanlan.zhihu.com/p/544766790
3 Run LIO-SAM for mapping
3.1 Modify the bag name
The name of the bag needs to be modified to the bag_filename variable in src/mapping/LIO-SAM/launch/run.launch.
3.2 Running LIO-SAM
Then run lio-sam, don't forget to source it before running.
roslaunch lio_sam run.launch
After that, the configured rviz interface will appear, as shown below:
4 Output trajectory for evo evaluation
To evaluate the mapping effect, you can compare the trajectory of the mapping with the true value provided by the dataset for accuracy analysis.
The code currently implements the function of outputting trajectory files. What evo needs is the trajectory in txt format, and the true value provided by the dataset is in csv format, so some conversion is required.
4.1 Convert the true value provided by the kaist dataset into tum format
This step needs to install dependencies
pip3 install numpy scipy
which have been filled in the install_dependence.sh file.
The execution steps are
./src/scripts/kaist2evo.py -p /media/trunk/Trunk/0-LX/Kaist/Urban08
-p is followed by the folder of the data set, and then you can add -o to add the address of the output trajectory file.
After execution, two txt files will be generated under the Urban08 folder, namely tum_ground_truth.txt and tum_vrs_gps.txt.
- tum_ground_truth.txt is a trajectory file in tum format converted from global_pose.csv
- tum_vrs_gps.txt is a trajectory file in tum format converted from sensor_data/vrs_gps.csv
Of course, I have already converted this, and put it in the src/doc/ground_truth folder, so I no longer need to convert it myself.
4.2 Perform evo trajectory comparison
When lio-sam finishes building the map, a pcd map will be generated under the Downloads/LOAM folder, and a trajectory file in tum format will be generated under the src/doc/mapping_results folder.
Then fill in the two files src/doc/ground_truth/tum_ground_truth.txt and src/doc/mapping_results/tum_lio_sam_pose.txt in txt1 and txt2 of src/scripts/evo.sh respectively
src/scripts/evo.shYou can draw trajectory diagrams, ape diagrams, and rpe diagrams during execution .
It can be seen that the deviation in the z direction is relatively large, and there is room for improvement in the future.
5 Changes to the code
5.1 Add a ring field
Since lio-sam needs the data of the ring field, but the vlp in the Kaist dataset does not have this field, then the ring field can be calculated by itself and then added to the point cloud.
float angle;
uint16_t ring;
while (!file.eof()) {
PointXYZIR point;
file.read(reinterpret_cast<char *>(&point.x), sizeof(float));
file.read(reinterpret_cast<char *>(&point.y), sizeof(float));
file.read(reinterpret_cast<char *>(&point.z), sizeof(float));
file.read(reinterpret_cast<char *>(&point.intensity), sizeof(float));
// 先计算角度，根据角度得到ring，然后将ring添加到点云中
angle = atan2(point.z, sqrt(point.x * point.x + point.y * point.y)) / M_PI * 180 + 15;
ring = round(angle / 2);
point.ring = ring;
pcl_cloud.points.push_back(point);
}
5.2 Comment out the time field check in lio-sam
Temporarily comment out the time field check in lio-sam. The timestamp of each point has not yet been considered how to do it.
Without the time field, naturally there is no motion distortion removal. Motion distortion removal is also commented out here.
5.3 Subscribe to the left and right radar data
The KAIST data set has two lidars, symmetrically distributed left and right, and the installation position is inclined to the ground, but the IMU installation position is parallel to the ground.
Previously, the original code of lio-sam only subscribed to one point cloud topic. When only one point cloud data of the Kaist dataset is used for map construction, the effect is not very good, so the lio-sam is changed to subscribe to two point cloud topics.
The specific code processing logic is as follows:
- Subscribe to 2 radar data
- Convert the external parameters of the left and right lidar to the IMU through the lidar to the IMU system to reduce the impact of other coordinate conversions
- Then complete the merger of two point clouds to get a range image
The specific code changes are as follows
Callback changed from 1 callback cloudHandler to 2 callbacks of pointCloudLeftHandler and pointCloudRightHandler
The right radar is the auxiliary radar, and its callback is as follows.
void pointCloudRightHandler(const sensor_msgs::PointCloud2ConstPtr &rightPointCloud) {
std::lock_guard<std::mutex> lock1(veloLock);
cachePointCloudRightQueue.push_back(*rightPointCloud);
if (cachePointCloudRightQueue.size() < 5) {
return;
}
currentPointCloudRightMsg = std::move(cachePointCloudRightQueue.front());
cachePointCloudRightQueue.pop_front();
pcl::moveFromROSMsg(currentPointCloudRightMsg, *pointCloudRightIn);
if (pointCloudRightIn->is_dense == false) {
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
pcl::PointCloud<PointXYZIRT>::Ptr pointCloudOut(new pcl::PointCloud<PointXYZIRT>());
pointCloudOut = transformPointCloud(pointCloudRightIn, rightLidarToImu);
pointCloudRightQueue.push_back(pointCloudOut);
return;
}
First cache the data, then convert it into pcl format, and then call the transformPointCloud function to transfer the radar from the radar coordinate system to the imu coordinate system.
The point cloud on the left is the main radar, which is the radar callback of lio-sam before, but there is an extra one, which is first transferred to the imu coordinate system and merged with the point cloud.
The code for merging point clouds is as follows
void mergePointCloud()
{
std::lock_guard<std::mutex> lock1(veloLock);
if(pointCloudLeftQueue.size() > 3 && pointCloudRightQueue.size() > 3)
{
pointCloudLeft = std::move(pointCloudLeftQueue.front());
pointCloudLeftQueue.pop_front();
pointCloudRight = std::move(pointCloudRightQueue.front());
pointCloudRightQueue.pop_front();
*pointCloudFull = *pointCloudLeft + *pointCloudRight;
}
else
{
ROS_DEBUG("Waiting for point cloud data ...");
return;
}
}
5.5 EKF node replacement
The original LIO-SAM uses EKF nodes to output Odometry for gps data conversion.
Now modify to use the raw GNSS data, convert it, and output the odometry for use by the mapOptmization node.
The order of conversion is as follows: LLA --> ECEF --> ENU, the schematic diagram of the coordinate system is as follows
Finally, the gps coordinates in the ENU system are obtained, and the code is as follows
// convert LLA to XYZ
Eigen::Vector3d lla = gtools.GpsMsg2Eigen(*msg);
Eigen::Vector3d ecef = gtools.LLA2ECEF(lla);
Eigen::Vector3d enu = gtools.ECEF2ENU(ecef);
It can also be replaced by the GeographicLib library, the code is very simple
GeographicLib::LocalCartesian geoConverter;
//初始化
if(!init)
{
geoConverter.Reset(gps_msg_ptr->latitude, gps_msg_ptr->longitude, gps_msg_ptr->altitude);
init = true;
}
geoConverter.Forward(gps_msg_ptr->latitude, gps_msg_ptr->longitude, gps_msg_ptr->altitude
5.6 Output Trajectory
In order to evaluate the mapping effect, it is necessary to output the mapping trajectory and compare it with the real value. So the function of outputting the optimized trajectory is added to the code.
The specific function is as follows
// 输出轨迹到txt
void saveGlobalPath()
{
std::string lio_sam_path = ros::package::getPath("lio_sam");
int npos_1 = lio_sam_path.find_last_of("/");
std::string path = lio_sam_path.substr(0, npos_1);
int npos_2 = path.find_last_of("/");
// src文件夹
std::string src_path = path.substr(0, npos_2);
std::ofstream tum_pose(src_path + "/doc/mapping_results" +
"/tum_lio_sam_pose.txt");
tum_pose.setf(std::ios::scientific, std::ios::floatfield);
for (auto ite = globalPath.poses.begin(); ite != globalPath.poses.end();
ite++) {
geometry_msgs::PoseStamped pose_stamped = *ite;
// tum格式的轨迹
tum_pose << pose_stamped.header.stamp << " "
<< pose_stamped.pose.position.x << " "
<< pose_stamped.pose.position.y << " "
<< pose_stamped.pose.position.z << " "
<< pose_stamped.pose.orientation.x << " "
<< pose_stamped.pose.orientation.y << " "
<< pose_stamped.pose.orientation.z << " "
<< pose_stamped.pose.orientation.w << std::endl;
}
tum_pose.close();
}
This function will be executed when the program ends, that is, it will be executed after pressing ctrl c. The directory to save the trajectory is in the src/doc/mapping_results folder, and the saved file name is tum_lio_sam_pose.txt.
5.7 Some other changes in the code
- Commented out the code for motion distortion removal
- Code changes were made to the prediction part of translation and rotation, but there are still some problems
- For the frequency of imu, whether to use the odometer, whether to use gps, add parameters to control whether to use
- Added the parameters of the external parameters of the left and right lidar to the IMU
6 Items to be optimized and realized
6.1 Items to be optimized
- Calculate the time field, and then remove the motion distortion
- There are still some problems with the code for the prediction part of the translation and rotation
- Solve the zero bias problem of imu, and estimate the zero bias at the beginning
- After the GPS constraint is added, it does not converge sporadically in the initial period of time. This is a bug to be fixed.
- There will be a height difference at the end of the Urban08 data set. At this time, the loopback cannot be found, so it cannot be optimized and needs to be resolved.
6.2 Items to be implemented
If any student has downloaded other data sets of Kaist, you can help to test them. For other data sets, you need to change the coordinate transformation from radar to imu in LIO-SAM/config/params.yaml. If you do, you can add me on WeChat , chat together.
Contributors
The code implementation of this article was completed by Zhao Huanfeng, Zhou Yong, and Li Wei. Thanks to these two students for their contributions.
epilogue
The code for this article has actually been written long ago, but the article has not been written. On the one hand, there are a lot of things at work, and I get off work late, and on the other hand, I am lazy.
The next plan is to run a positioning through this not-so-perfect map.
Let's start with the integration of odom and imu.