自己用点云分割的预测结果，和里程计的结果拼接出整个轨迹，某种程度上也算是语义语义地图
只需要pcl c++就可以，参了以下博主
kitti点云地图拼接_kitti点云拼接-CSDN博客
SemanticKITTI点云拼接+PCL可视化_可视化semantickitti .bin-CSDN博客
代码
#include <pcl/io/pcd_io.h> // pcl::io::savePCDFileBinary
#include <pcl/common/transforms.h> // pcl::transformPointCloud
#include <pcl/registration/ndt.h> // voxelGridFilter
#include <pcl/filters/statistical_outlier_removal.h>
#include <unordered_map>
// #include <tf/tf.h> // tf::Matrix3x3, tf::createQuaternionMsgFromRollPitchYaw, tf::Quaternion
// #include <ros/ros.h>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <boost/filesystem.hpp>
typedef pcl::PointXYZL pointType;
static Eigen::Matrix4f tform;
std::vector<Eigen::Matrix4f> tforms;
std::string save_map_name = "output_map.pcd";
std::string filter_flag = "filter";
std::string label_path;
std::string bin_path, poses_file;
std::vector<std::string> bin_names;
std::vector<std::string> label_names;
pcl::PointCloud<pointType>::Ptr source_cloud (new pcl::PointCloud<pointType> ());
pcl::PointCloud<pointType>::Ptr transformed_cloud (new pcl::PointCloud<pointType> ());
pcl::PointCloud<pointType>::Ptr map_cloud (new pcl::PointCloud<pointType> ());
std::unordered_map<int,int> label_map={
{ 0 , 0 } ,
{ 1 , 0 } ,
{ 10 , 1 } ,
{ 11 , 2 } ,
{ 13 , 5 } ,
{ 15 , 3 } ,
{ 16 , 5 } ,
{ 18 , 4 } ,
{ 20 , 5 } ,
{ 30 , 6 } ,
{ 31 , 7 } ,
{ 32 , 8 } ,
{ 40 , 9 } ,
{ 44 , 10 } ,
{ 48 , 11 } ,
{ 49 , 12 } ,
{ 50 , 13 } ,
{ 51 , 14 } ,
{ 52 , 0 } ,
{ 60 , 9 } ,
{ 70 , 15 } ,
{ 71 , 16 } ,
{ 72 , 17 } ,
{ 80 , 18 } ,
{ 81 , 19 } ,
{ 99 , 0 } ,
{ 252 , 1 } ,
{ 253 , 7 } ,
{ 254 , 6 } ,
{ 255 , 8 } ,
{ 256 , 5 } ,
{ 257 , 5 } ,
{ 258 , 4 } ,
{ 259 , 5 }
};
std::unordered_map<int,int> learning_map_inv={
{ 0 , 0 } ,
{ 1 , 10 } ,
{ 2 , 11 } ,
{ 3 , 15 } ,
{ 4 , 18 } ,
{ 5 , 20 } ,
{ 6 , 30 } ,
{ 7 , 31 } ,
{ 8 , 32 } ,
{ 9 , 40 } ,
{ 10 , 44 } ,
{ 11 , 48 } ,
{ 12 , 49 } ,
{ 13 , 50 } ,
{ 14 , 51 } ,
{ 15 , 70 } ,
{ 16 , 71 } ,
{ 17 , 72 } ,
{ 18 , 80 } ,
{ 19 , 81 }
};
bool vstring_compare(const std::string &x,const std::string &y) //&符号不能少
{
return x < y;
}
//find bin list
void get_bin_names(const std::string& root_path, std::vector<std::string> &names)
{
names.clear();
boost::filesystem::path full_path(root_path);
boost::filesystem::recursive_directory_iterator end_iter;
for(boost::filesystem::recursive_directory_iterator iter(full_path); iter!=end_iter; ++iter)
{
try
{
if ( !boost::filesystem::is_directory( *iter ) )
{
std::string file = iter->path().string();
names.push_back(iter->path().string()); // get the golbal full path name. 获取该文件的全局路径
// boost::filesystem::path file_path(file);
// names.push_back(file_path.stem().string()); // get the pure name(no suffix) 获取无后缀的文件名称，即000000, 000001, ...
}
}
catch ( const std::exception & ex )
{
std::cerr << ex.what() << std::endl;
continue;
}
}
}
//find label list
void get_label_names(const std::string& root_path, std::vector<std::string> &names)
{
names.clear();
boost::filesystem::path full_path(root_path);
boost::filesystem::recursive_directory_iterator end_iter;
for(boost::filesystem::recursive_directory_iterator iter(full_path); iter!=end_iter; ++iter)
{
try
{
if ( !boost::filesystem::is_directory( *iter ) )
{
std::string file = iter->path().string();
names.push_back(iter->path().string()); // get the golbal full path name. 获取该文件的全局路径
// boost::filesystem::path file_path(file);
// names.push_back(file_path.stem().string()); // get the pure name(no suffix) 获取无后缀的文件名称，即000000, 000001, ...
}
}
catch ( const std::exception & ex )
{
std::cerr << ex.what() << std::endl;
continue;
}
}
}
// 解析得到对应的变换矩阵，并将方向转换到lidar所在坐标系下：
void get_transforms(std::string pose_file, std::vector<Eigen::Matrix4f>& tforms)
{
std::string line;
std::ifstream ifs;
ifs.open(pose_file, std::ios::in);
if (!ifs)
{
std::cout << "cannot open file: " << pose_file << std::endl;
return ;
}
while (std::getline(ifs, line) && ifs.good())
{
if (line.empty()) return;
std::stringstream lineStream(line);
std::string cell;
std::vector<double> vdata;
while (std::getline(lineStream, cell, ' '))
{
vdata.push_back(std::stod(cell));
}
Eigen::Matrix4f tform;
tform(0, 0) = vdata[0]; tform(0, 1) = vdata[1]; tform(0, 2) = vdata[2]; tform(0, 3) = vdata[3];
tform(1, 0) = vdata[4]; tform(1, 1) = vdata[5]; tform(1, 2) = vdata[6]; tform(1, 3) = vdata[7];
tform(2, 0) = vdata[8]; tform(2, 1) = vdata[9]; tform(2, 2) = vdata[10]; tform(2, 3) = vdata[11];
tform(3, 0) = 0; tform(3, 1) = 0; tform(3, 2) = 0; tform(3, 3) = 1;
Eigen::Matrix4f t;
t<<-1.857739385241e-03, -9.999659513510e-01, -8.039975204516e-03,-4.784029760483e-03,\
-6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02,\
9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01,\
0 , 0 , 0, 1;
Eigen::Matrix4f t_inverse = t.inverse();
tform = t_inverse*tform*t;
// Eigen::Matrix4f tform = Eigen::Matrix4f::Identity();
// Eigen::Matrix3f tf_mat; // camera旋转矩阵
// tf_mat << vdata[0], vdata[1], vdata[2], vdata[4], vdata[5], vdata[6], vdata[8], vdata[9], vdata[10];
// // Eigen::Vector3f trans_camera(vdata[3], vdata[7], vdata[11]);
// Eigen::Vector3f trans_camera(vdata[11], -vdata[3], -vdata[7]);
// Eigen::Quaternionf q_camera(tf_mat);
// Eigen::Quaternionf q_lidar(q_camera.w(), q_camera.z(), -q_camera.x(), -q_camera.y());
// Eigen::Matrix3f R_lidar = q_lidar.toRotationMatrix();
// tform.block<3,3>(0,0) = R_lidar;
// tform.block<3,1>(0,3) = trans_camera;
tforms.push_back(tform);
// static int count = 0;
// std::cout << count++ << "transform: \n" << tform << std::endl;
}
}
// 解析.bin格式的点云文件，转换到标准的pcl::PointXYZI的格式：
/*
void parse_bin_cloud(const std::string& bin_file, pcl::PointCloud<pointType>& points)
{
points.points.clear();
std::fstream input(bin_file.c_str(), std::ios::in | std::ios::binary);
if(!input.good())
{
std::cerr << "Could not read file: " << bin_file << std::endl;
exit(EXIT_FAILURE);
}
// bin2points:
input.seekg(0, std::ios::beg);
for (int i=0; input.good() && !input.eof(); i++)
{
pointType point;
input.read((char *) &point.x, 3*sizeof(float));
input.read((char *) &point.intensity, sizeof(float));
points.points.push_back(point);
}
input.close();
}
*/
// 解析.bin格式的点云文件，转换到标准的pcl::PointXYZL
int loadBin_with_Label(
const std::string& binName, const std::string& labelName,
pcl::PointCloud<pcl::PointXYZL>& p_cloud_l) {
p_cloud_l.points.clear();
int32_t num = 1000000;
float* data = (float*)malloc(num * sizeof(float));
uint32_t* data_label = (uint32_t*)malloc(num * sizeof(uint32_t));
// 点
float* px = data + 0;
float* py = data + 1;
float* pz = data + 2;
float* pr = data + 3; //反射强度
uint32_t* pl = data_label + 0; // label
FILE* stream;
FILE* label_stream;
const char* filenameInput = binName.c_str();
const char* labelnameInput = labelName.c_str();
// fopen_s(&stream, filenameInput, "rb"); // fopen_s是微软的版本
stream = fopen(filenameInput, "rb");
label_stream = fopen(labelnameInput, "rb");
num = fread(data, sizeof(float), num, stream) /4; //读入点云数据，大概10万+个点
auto r = fread(data_label, sizeof(uint32_t), num, label_stream);
// std::cout << *data_label << std::endl;
// std::cout << "num: " << num << ", label num: " << r << std::endl;
// 创建点云
// pcl::PointCloud<pcl::PointXYZI>::Ptr p_cloud_i(new
// pcl::PointCloud<pcl::PointXYZI>());
for (size_t i = 0; i < num; i++) {
pcl::PointXYZL tmp;
tmp.x = *px;
tmp.y = *py;
tmp.z = *pz;
uint32_t upper_half = *pl >> 16;
uint32_t lower_half = (*pl) & 0xffff;
uint32_t label = upper_half << 16 + lower_half;
lower_half = lower_half & 0x0000ffff;
tmp.label = learning_map_inv[label_map[lower_half]];
//todo
// p_cloud_l->push_back(tmp);
p_cloud_l.points.push_back(tmp);
px += 4;
py += 4;
pz += 4;
pr += 4;
pl += 1;
}
fclose(stream);
fclose(label_stream);
return 1;
}
// 对点云进行将采用，减小对系统资源的占用，加快程序运行：
void voxel_grid_filter(const pcl::PointCloud<pointType>::Ptr& source_cloud, pcl::PointCloud<pointType>::Ptr& filtered_cloud, const double& voxel_leaf_size)
{
pcl::VoxelGrid<pointType> voxel_filter;
voxel_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
voxel_filter.setInputCloud(source_cloud);
voxel_filter.filter(*filtered_cloud);
return;
}
// 拼接点云地图
void joint_map(const std::vector<std::string>& bin_names, const std::vector<Eigen::Matrix4f>& tforms,const std::vector<std::string>& label_names)
{
long long int cnt=0;
for (int i = 0; i < bin_names.size(); ++i)
{
// convert kitti lidar data *.bin to pcl pointcloud type:
// parse_bin_cloud(bin_names[i], *source_cloud);
loadBin_with_Label(bin_names[i], label_names[i],*source_cloud); //load_seg
// std::cout <<i<<"get points: " << source_cloud->points.size() << std::endl;
cnt += source_cloud->points.size();
// transform the point cloud:
pcl::transformPointCloud(*source_cloud, *transformed_cloud, tforms[i]);
// //统计滤波
if(filter_flag == "filter")
{
// std::cout<<i<<" before:"<<source_cloud->points.size()<<std::endl;
pcl::StatisticalOutlierRemoval<pointType> sor;
// 设置滤波器参数
sor.setInputCloud(source_cloud);
sor.setMeanK(50); // 设置用于计算统计信息的临近点数
sor.setStddevMulThresh(1.0); // 设置标准差的倍数阈值
sor.filter(*source_cloud);
// std::cout<<i<<" after:"<<source_cloud->points.size()<<std::endl;
}
// voxel grid filter the cloud:
if(filter_flag == "filter")
voxel_grid_filter(transformed_cloud, transformed_cloud, 0.2);
// 删除指定label的点云 0 未知
// for (size_t i = 0; i < transformed_cloud->points.size(); ++i) {
// // 如果点的标签为10，则删除该点
// if (transformed_cloud->points[i].label == 10) {
// transformed_cloud->points.erase(transformed_cloud->points.begin() + i);
// // 删除点后，需要将索引回退一步
// --i;
// }
// }
if (i == 0)
{
*map_cloud = *transformed_cloud;
continue;
}
*map_cloud += *transformed_cloud;
}
// voxel grid filter the output map:
if(filter_flag == "filter")
voxel_grid_filter(map_cloud, map_cloud, 0.9);
//统计滤波
if(filter_flag == "filter")
{
// std::cout<<" before:"<<map_cloud->points.size()<<std::endl;
pcl::StatisticalOutlierRemoval<pointType> sor;
// 设置滤波器参数
sor.setInputCloud(map_cloud);
sor.setMeanK(50); // 设置用于计算统计信息的临近点数
sor.setStddevMulThresh(1.5); // 设置标准差的倍数阈值
sor.filter(*map_cloud);
// std::cout<<" after:"<<map_cloud->points.size()<<std::endl;
}
// save pointcloud to pcd:
pcl::io::savePCDFileBinary(save_map_name, *map_cloud);
// show save map info:
std::cout << "the map " << save_map_name << " is saved with " << map_cloud->points.size() << " points" <<" // "<<cnt<< std::endl;
}
int main(int argc, char** argv)
{
if (argc < 3)
{
std::cout << "Usage: bin_name bin_path pose_name output_map_name(optional). " << std::endl;
return 0;
}
bin_path = argv[1];
poses_file = argv[2];
save_map_name = argv[3];
label_path = argv[4];
filter_flag = argv[5];
// ./concat 点云数据文件 位姿文件 输出文件名字 label文件
get_bin_names(bin_path, bin_names); // get the kitti lidar bin file names.
sort(bin_names.begin(), bin_names.end(), vstring_compare); // sort the names.（由于get_bin_names函数得到的文件路径是无序的，这里将它按照000000.bin, 000001.bin的顺序排列好）
get_label_names(label_path, label_names); // get the kitti lidar bin file names.
sort(label_names.begin(), label_names.end(), vstring_compare); // sort the names.（由于get_bin_names函数得到的文件路径是无序的，这里将它按照000000.bin, 000001.bin的顺序排列好）
get_transforms(poses_file, tforms); // get the kitti global poses and convert it to transform: Eigen::Matrix4f.
// for (size_t i = 0; i < bin_names.size(); ++i) // print the bin name and its affine matrix.
// {
// std::cout << bin_names[i] << std::endl;
// std::cout << tforms[i] << std::endl;
// }
joint_map(bin_names, tforms, label_names); // joint the map.
return 0;
}
可视化结果
PCDViewer Release Page-CSDN博客
也可以用https://www.cloudcompare.org/
两个软件设置颜色略有不同，不过按照官方的颜色直接设置就行了，最终效果如下。
整体上看差不多，和原始论文差不多，由于自己建图的时候采用的前视角的点云和里程计的数据，因此有一点不准，如果直接使用原始的标签和位姿结果应该和原论文差不多。
