c++ plot画图与无序点云曲线拟合

在HD Map制作过程中,针对提取出来的车道线、路沿等线状点云,本身是无序的,在存储的时候需要先进行曲线拟合,然后采样排序。本篇博客主要就车道线点云在2D/3D空间下的拟合,做一次整理,并给出源码,可作为工具使用。

image-20200605092148239

这里提供2D直线拟合、2D多项式拟合、3D分段质心拟合这几种最小二乘方法,需要扩展的话在相应的类里面添加函数即可。

image-20200605092207922

数据读取

这里我们读取点云数据,将其x、y、z坐标分别存到vector中,方便后续处理。

1
2
3
4
5
6
7
8
9
10
11
12
13
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::io::loadPCDFile("../pcd_dir/17.pcd", *cloud); // 加载单帧的车道线点云

std::vector<double> X, Y, Z;
for (int i = 0; i < cloud->size(); i++) {
X.push_back(cloud->points[i].x);
Y.push_back(cloud->points[i].y);
Z.push_back(cloud->points[i].z);
}

std::vector<double> x_mean, y_mean, z_mean;
pcl::PointCloud<PointT>::Ptr point_mean(new pcl::PointCloud<PointT>);// 存储均值点云
double a, b, c, k_line, b_line; // k、b表示直线斜率和截距

2D拟合

首先设置工具类,定义初始点云。

1
2
fitting_line::FittingLine fittingLine_;
fittingLine_.setInputcloud(cloud);

调用直线拟合或者多项式拟合函数。

1
2
3
4
5
6
7
//  2D平面直线拟合
fittingLine_.line_fitting(X, Y, k_line, b_line);// 2D直线拟合
fittingLine_.display_line(X, Y, b_line, k_line);// 显示拟合的直线,必须先输入常量

// 2D 平面多项式拟合
fittingLine_.polynomial2D_fitting(X, Y, a, b, c); // 2D多项式拟合
fittingLine_.display_line(X, Y, c, b, a);// 显示拟合的平面多项式曲线,输入顺序为 常量,一阶系数,二阶系数

根据最小二乘准则,来求解拟合的多项式系数。最小二乘法参考下面博客:

[https://blog.csdn.net/qq_29831163/article/details/89507997#1%20%C2%A0%E7%BA%BF%E6%80%A7%E6%9C%80%E5%B0%8F%E4%BA%8C%E4%B9%98%E6%B3%95%C2%A0](https://blog.csdn.net/qq_29831163/article/details/89507997#1 线性最小二乘法 )

img

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
// 一阶 
void FittingLine::line_fitting(std::vector<double> x, std::vector<double> y, double &k, double &b) {
Eigen::MatrixXd A_(2, 2), B_(2, 1), A12(2, 1);
int num_point = x.size();
double A01(0.0), A02(0.0), B00(0.0), B10(0.0);
for (int i_point = 0; i_point < num_point; i_point++) {
A01 += x[i_point] * x[i_point];
A02 += x[i_point];
B00 += x[i_point] * y[i_point];
B10 += y[i_point];
}
A_ << A01, A02,
A02, num_point;
B_ << B00,
B10;
A12 = A_.inverse() * B_;
k = A12(0, 0);
b = A12(1, 0);
}

// 二阶
void FittingLine::polynomial2D_fitting(std::vector<double> x, std::vector<double> y, double &a, double &b, double &c) {
Eigen::MatrixXd A_(3, 3), B_(3, 1), A123(3, 1);
int num_point = x.size();
double A01(0.0), A02(0.0), A12(0.0), A22(0.0), B00(0.0), B10(0.0), B12(0.0);
for (int i_point = 0; i_point < num_point; i_point++) {
A01 += x[i_point];
A02 += x[i_point] * x[i_point];
A12 += x[i_point] * x[i_point] * x[i_point];
A22 += x[i_point] * x[i_point] * x[i_point] * x[i_point];
B00 += y[i_point];
B10 += x[i_point] * y[i_point];
B12 += x[i_point] * x[i_point] * y[i_point];
}
A_ << num_point, A01, A02,
A01, A02, A12,
A02, A12, A22;
B_ << B00,
B10,
B12;
A123 = A_.inverse() * B_; // 求解参数
a = A123(2, 0);
b = A123(1, 0);
c = A123(0, 0);
}

可视化采用PCLPlotter来完成。

1
2
3
4
5
6
7
8
9
10
11
void FittingLine::display_line(std::vector<double> vector_1, std::vector<double> vector_2, double c, double b, double a) {
pcl::visualization::PCLPlotter *plot_line1(new pcl::visualization::PCLPlotter);
std::vector<double> func1(3, 0);
func1[0] = c;
func1[1] = b;
func1[2] = a;
plot_line1->addPlotData(func1, point_min_.x, point_max_.x); // 参数、起始x坐标
plot_line1->addPlotData(vector_1, vector_2, "display", vtkChart::POINTS);//X,Y均为double型的向量
plot_line1->setShowLegend(false);
plot_line1->plot();
}

3D拟合

这里主要采用分段质心的拟合方法。

1
2
3
4
5
 // 3D分段质心拟合
fittingLine_.grid_mean_xyz(0.5, 0, x_mean, y_mean, z_mean, point_mean);
fittingLine_.grid_mean_xyz_display(point_mean);// 展示均值拟合结果
fittingLine_.display_point(X, Y);// 显示散点
fittingLine_.display_point(x_mean, y_mean);// 显示均值散点

0.5表示x方向的步长,-1(小于0就行)表示y方向不分段,如需分段,则设置相应步长。可以得到每一段的质心,然后对这些质心点进行拟合即可。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
void FittingLine::grid_mean_xyz(double x_resolution, double y_resolution, std::vector<double> &x_mean, std::vector<double> &y_mean,std::vector<double> &z_mean, pcl::PointCloud<PointT>::Ptr &new_cloud) {
if (y_resolution <= 0) {
y_resolution = point_max_.y - point_min_.y; //不分段
}
int raster_rows, raster_cols;
raster_rows = ceil((point_max_.x - point_min_.x) / x_resolution);
raster_cols = ceil((point_max_.y - point_min_.y) / y_resolution);
std::vector<int> idx_point;
std::vector<std::vector<std::vector<float>>> row_col;
std::vector<std::vector<float>> col_;
std::vector<float> vector_4;
vector_4.resize(4);
col_.resize(raster_cols, vector_4);
row_col.resize(raster_rows, col_);
int point_num = cloud_->size();
for (int i_point = 0; i_point < point_num; i_point++) {
int row_idx = ceil((cloud_->points[i_point].x - point_min_.x) / x_resolution) - 1;
int col_idx = ceil((cloud_->points[i_point].y - point_min_.y) / y_resolution) - 1;
if (row_idx < 0)row_idx = 0;
if (col_idx < 0)col_idx = 0;
row_col[row_idx][col_idx][0] += cloud_->points[i_point].x;
row_col[row_idx][col_idx][1] += cloud_->points[i_point].y;
row_col[row_idx][col_idx][2] += cloud_->points[i_point].z;
row_col[row_idx][col_idx][3] += 1;
}
PointT point_mean_tem;
for (int i_row = 0; i_row < row_col.size(); i_row++) {
for (int i_col = 0; i_col < row_col[i_row].size(); i_col++) {
if (row_col[i_row][i_col][3] != 0) {
double x_mean_tem = row_col[i_row][i_col][0] / row_col[i_row][i_col][3];
double y_mean_tem = row_col[i_row][i_col][1] / row_col[i_row][i_col][3];
double z_mean_tem = row_col[i_row][i_col][2] / row_col[i_row][i_col][3];
x_mean.push_back(x_mean_tem);
y_mean.push_back(y_mean_tem);
z_mean.push_back(z_mean_tem);
point_mean_tem.x = x_mean_tem;
point_mean_tem.y = y_mean_tem;
point_mean_tem.z = z_mean_tem;
new_cloud->push_back(point_mean_tem);
}
}
}
}

可视化这里用PCL viewer。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void FittingLine::grid_mean_xyz_display(pcl::PointCloud<PointT>::Ptr new_cloud) {
pcl::visualization::PCLVisualizer::Ptr view(new pcl::visualization::PCLVisualizer("分段质心拟合"));
pcl::visualization::PointCloudColorHandlerCustom<PointT> color_1(new_cloud, 255, 0, 0);
view->addPointCloud(new_cloud, color_1, "11");
view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "11");
pcl::PointCloud<PointT>::Ptr new_cloud_final(new pcl::PointCloud<PointT>);
for (int i_point = 0; i_point < cloud_->size(); i_point++) {
PointT tem_point;
tem_point.x = cloud_->points[i_point].x;
tem_point.y = cloud_->points[i_point].y;
tem_point.z = cloud_->points[i_point].z;
new_cloud_final->push_back(tem_point);
}
view->addPointCloud(new_cloud_final, "22");
view->spin();
}

用PCL Plot画散点图。

1
2
3
4
5
6
void FittingLine::display_point(std::vector<double> vector_1, std::vector<double> vector_2) {
pcl::visualization::PCLPlotter *plot_line1(new pcl::visualization::PCLPlotter);
plot_line1->addPlotData(vector_1, vector_2, "display", vtkChart::POINTS);//X,Y均为double型的向量
plot_line1->setShowLegend(false);
plot_line1->plot();
}

目前只是一个测试的工具类吧,后续有实际用到的较好的方法再来更新。

在路网构建上的应用

其实以上的一些曲线拟合方法很常见,这里主要考虑在高精地图制图流程中的应用,单条车道线的拟合。在每一条车道线拟合完成之后,我们可以记录其起始点和采样点,然后可以根据录制的GPS轨迹点识别路口,差不多是一个局部的检索算子和霍夫算子原理差不多,这样就可以得到地图图层,生成路网。这部分的实际例子,后续补充。

-------------    本文结束  感谢您的阅读    -------------
胡想成 wechat
欢迎关注个人公众号!
您的支持,是我装逼的最大动力!

本文标题:c++ plot画图与无序点云曲线拟合

文章作者:胡想成

发布时间:2020年06月05日 - 09:06

最后更新:2020年06月05日 - 17:06

原始链接:xchu.net/2020/06/05/47plot/

版权声明:本博客所有文章除特别声明外,均采用 CC BY-NC-SA 4.0许可协议,转载请注明出处!