pcl通过室内点云计算房间参数

获取点云文件

拿到的是.obj文件,使用

pcl_mesh_sampling_release Model.obj 1.pcd

将其转换为点云格式
使用pcl_viewer_release.exe打开,按2以看得更清楚.

下采样

对点云进行下采样,下采样后进行平面提取的效果更好而且更快.

void down_sample(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    cout << "正在进行下采样" << endl;
    pcl::VoxelGrid<pcl::PointXYZ> voxel;
    voxel.setInputCloud(cloud);
    voxel.setLeafSize(0.1f, 0.1f, 0.1f);// 设置体素大小
    voxel.filter(*cloud);
}

计算质心

计算点云质心位置,这个后面会用到

Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);
cout << "点云质心("
    << centroid[0] << ","
    << centroid[1] << ","
    << centroid[2] << ")." << endl;

点云质心(4.49549,4.98694,5.38814)

提取平面

void segment(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& seg_clouds,
    vector<pcl::ModelCoefficients::Ptr>& seg_coefficients, int faces)
{
    cout << "正在准备表面提取" << endl;
    // 复制点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*input_cloud, *cloud);
    // 提取表面
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    // 参数
    seg.setOptimizeCoefficients(true);
    // 可选参数
    seg.setModelType(pcl::SACMODEL_PLANE);//所提取目标模型的属性（平面、球、圆柱等等）
    seg.setMethodType(pcl::SAC_RANSAC);//采样方法（RANSAC、LMedS等）
    seg.setDistanceThreshold(0.1);//查询点到目标模型的距离阈值（如果大于此阈值，则查询点不在目标模型上，默认值为0）。
    seg.setMaxIterations(100);//最大迭代次数（默认值为50）
    // seg.setProbability(.99);//至少一个样本不包含离群点的概率（默认值为0.99）

    // 提取索引
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    seg_clouds.clear();
    seg_coefficients.clear();

    for (int i = 0;i < faces;i++) 
    {
        cout << "正在提取第" << i + 1 << "个表面" << endl;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        seg.setInputCloud(cloud);
        seg.segment(*inliers, *coefficients);

        extract.setInputCloud(cloud);
        extract.setIndices(inliers);// 设置分割后的内点为需要提取的点集
        extract.setNegative(false);// 设置提取内点
        extract.filter(*cloud_filtered);// 提取并保存
        extract.setNegative(true);
        extract.filter(*cloud);

        seg_clouds.push_back(cloud_filtered);
        seg_coefficients.push_back(coefficients);
    }
    seg_clouds.push_back(cloud);

    cout << "表面提取完成" << endl;
    return;
}

理论上setDistanceThreshold设置得越低精度越高,但是当点云的精度本身就比较低时threshold设置很低的话将不能很好地分隔出平面.

我们先提取15个面,其实主要需要的是平面参数.

// 提取平面
vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
vector<pcl::ModelCoefficients::Ptr> seg_coefficients;
segment(cloud, seg_clouds, seg_coefficients, 15);

// 输出平面参数
cout << "Coefficient" << endl;
for (int i = 0; i < seg_coefficients.size(); i++)
{
    cout << seg_coefficients[i]->values[0] << " "
        << seg_coefficients[i]->values[1] << " "
        << seg_coefficients[i]->values[2] << " "
        << seg_coefficients[i]->values[3] << ";";
}
cout << endl;

Coefficient
0.0240403 -0.999651 0.0109361 6.17904;-0.000307209 -0.999907 0.0136617 3.39894;0.989412 0.0303559 -0.141925 -5.88116;0.999448 -0.00687066 -0.0325001 -2.25654;0.0824583 0.0277749 0.996207 -3.04236;-0.0986582 0.0587155 0.993388 -7.78671;0.0347947 -0.995243 0.0909932 5.72497;0.944357 0.0330723 -0.327254 -4.42751;-0.0222358 -0.995385 0.0933517 3.09012;-0.111973 0.120821 0.986339 -7.36444;0.976855 0.0615073 0.204869 -7.85129;0.856399 -0.0602228 -0.512791 1.17163;-0.00967404 0.164111 .986394 -8.09556;-0.0655087 0.0912942 0.993667 -8.22552;0.999101 0.0137901 0.0400977 -2.72693;

平面提取可视化

我们可以先使用以下两个函数来观察平面提取的效果并分别保存每个面的点云.

// 可视化点云集合
void visual_clouds(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds) 
{
    cout << "开始可视化点云集合" << endl;
    pcl::visualization::PCLVisualizer viewer("pointcloud viewer");
    for (int i = 0;i < clouds.size();i++)
    {
        int *rgb = rand_rgb();//随机生成0-255的颜色值
        // 最后一组点云设置为白色
        if (i == clouds.size() - 1) 
        {
            rgb[0] = 255;
            rgb[1] = 255;
            rgb[2] = 255;
        }
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sig(clouds[i], rgb[0], rgb[1], rgb[2]);
        viewer.addPointCloud(clouds[i], sig, "cloud" + std::to_string(i));
    }
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
}

// 保存点云集合
void save_clouds(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds, string path="cloud") 
{
    cout << "正在储存点云集合到" << path << "_?.pcd" << endl;
    for (int i = 0;i < clouds.size();i++)
    {
        pcl::io::savePCDFileBinary(path + "_" + std::to_string(i) + ".pcd", *clouds[i]);
    }
    cout << "点云储存完毕" << endl;
}

图为提取7个平面和剩余点云

参数计算

现在我们有了点云的质心和分割出来15个平面的参数.
方便起见先用python进行数学运算.

# 质心参数
cent=array([4.49549,4.98694,5.38814])
cent=np.append(cent,1)
# 切片参数
coe=array(mat("0.0240403 -0.999651 0.0109361 6.17904;-0.000307209 -0.999907 0.0136617 3.39894;0.989412 0.0303559 -0.141925 -5.88116;0.999448 -0.00687066 -0.0325001 -2.25654;0.0824583 0.0277749 0.996207 -3.04236;-0.0986582 0.0587155 0.993388 -7.78671;0.0347947 -0.995243 0.0909932 5.72497;0.944357 0.0330723 -0.327254 -4.42751;-0.0222358 -0.995385 0.0933517 3.09012;-0.111973 0.120821 0.986339 -7.36444;0.976855 0.0615073 0.204869 -7.85129;0.856399 -0.0602228 -0.512791 1.17163;-0.00967404 0.164111 .986394 -8.09556;-0.0655087 0.0912942 0.993667 -8.22552;0.999101 0.0137901 0.0400977 -2.72693"))

# 计算两个平面的夹角
def angle_plan(a,b):
    def s2(a):
        return sqrt(a[0]**2+a[1]**2+a[2]**2)
    c=sum((a*b)[:3])/s2(a)/s2(b)
    return arccos(c)*180/pi


# 构造夹角矩阵(显示用)
def angle_mat(coe):
    size=coe.shape[0]
    newmat=zeros((size,size))
    for i in range(size):
        for j in range(i,size):
            newmat[i,j] = angle_plan(coe[i],coe[j])
    return newmat

# 将切片分类
def group_wall(coe):
    size=coe.shape[0]
    team=[]
    free=[i for i in range(size)]
    # 添加切片(递归)
    def app(i):
        free.remove(i)
        team[-1].append(i)
        for j in range(i+1,size):
            if j in free and angle_plan(coe[i],coe[j])<20:
                app(j)
    for i in range(size):
        if i in free:
            team.append([])
            app(i)
    if len(free):
        print("Error: Free doesn't clear \n")
        print(free)
    if len(team) < 3:
        print("group not enough:", len(group))
    return team

# 取每组中最平行的切片(废弃)
def group_best(coe, team):
    best_mach=[]
    for t in team:
        best_mach.append([])
        best_mach_num=180
        for i in range(len(t)):
            for j in range(i+1,len(t)):
                if angle_plan(coe[t[i]],coe[t[j]])<best_mach_num:
                    best_mach_num=angle_plan(coe[t[i]],coe[t[j]])
                    best_mach[-1]=[t[i],t[j]]
    return best_mach

# 求三维向量的模
def module(vector):
    return sqrt(vector[0]**2+vector[1]**2+vector[2]**2)

# 点到平面距离
def dis_dp(dot,plan):
    return sum(dot*plan)/module(plan)

# 根据选取的面计算房间长宽高
def distance_wall(dot,coe,group):
    distance=[]
    for g in group[:3]:
        distance.append(abs(dis_dp(dot,coe[g[0]])-dis_dp(dot,coe[g[1]])))
    return distance

# 主函数(质心,切片)
def main(cent,coe):
    group=group_wall(coe)
    dis=distance_wall(cent,coe,group)
    print(dis)

先使用group_wall计算出夹角小于20°的平面,可以看见15个平面刚好被分成了3组.

[[0, 1, 6, 8], [2, 3, 7, 11, 10, 14], [4, 5, 9, 12, 13]]

使用distance_wall计算每组前两个平面的距离(由于分割的时候会先分割大的平面,所以靠前的平面通常都是墙壁)

[2.876144394974303, 4.073686734342731, 5.419447930288962]

结果

最后得出该室内模型的参数
高2.876144394974303
宽4.073686734342731
长5.419447930288962

扫描的房间用卷尺量的数据：长5.3米，宽3.78米，高2.9米

后记

对原始点云尝试过最小二乘平滑,剔除离群点,高斯卷积操作,均不能提升平面提取效果.
需要对包含家具的室内点云进行测试.
需要评估和提升该方法的精度.