Python-pcl 随机采样一致性算法

RANSAC 随机采样一致性算法

RANSAC是一种随机参数估计算法。RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，在使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点（inliers），简称局内点或内点。否则为模型外样本点（outliers）。
然后迭代，取inliers的个数最多的作为结果返回；

PCL中Sample_Consensus支持的几种几何模型有：

SACMODEL_PLANE:平面模型

SACMODEL_LINE：直线模型

SACMODEL_CIRCLE2D：二维圆的圆周模型

SACMODEL_SPHERE：三维球体模型

SACMODEL_CYLINDER：圆柱体模型

SACMODEL_CONE：圆锥模型，尚未实现

SACMODEL_TORUS: 圆环面模型，尚未实现

SACMODEL_PARALLEL_LINE：有条件的直线模型，直线模型与给定轴线平行

SACMODEL_PERPENDICULAR_PLANE：有条件限制的平面模型，平面模型与给定轴线垂直

SACMODEL_NORMAL_PLANE: 有条件限制的平面模型，第一个局内点的发现必须与估计的平面模型的法线平行

SACMODEL_PARALLEL_PLANE: 有条件限制的平明模型在规定的最大角度偏差限制下，平面模型与给定的轴线平行

SACMODEL_NORMAL_PARLLEL_PLANE:有条件限制的平面模型三维平面模型与用户设定的轴线平行

下边的例子演示了
无参数时原始点云平面内点与立方体外点
参数为-f 提取得到的平面上的内点
参数为-s 提取得到圆球内点与立方体外点
参数为 -sf 提取圆球上的点为内点

无参数
在这里插入图片描述 参数为-f

参数为-s
在这里插入图片描述 参数为-sf

# -*- coding: utf-8 -*-
# How to use Random Sample Consensus model
# http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus

import numpy as np
import random
import pcl.pcl_visualization
import math
import sys

argvs = sys.argv  # for random model
# argvs = '-f' # plane model
# argvs = '-sf' # sphere model
argc = len(argvs)  # 参数个数
print(argc)
print('argvs: ',argvs)

# 初始化点云
cloud = pcl.PointCloud()
final = pcl.PointCloud()

points = np.zeros((5000, 3), dtype=np.float32)
RAND_MAX = 1024

for i in range(0, 5000):
    if argc > 1:
        if argvs[1] == "-s" or argvs[1] == "-sf":
            points[i][0] = 1024 * random.random() / (RAND_MAX + 1.0)
            points[i][1] = 1024 * random.random() / (RAND_MAX + 1.0)
            if i % 5 == 0:
                points[i][2] = 1024 * random.random() / (RAND_MAX + 1.0)
            elif i % 2 == 0:
                points[i][2] = math.sqrt(math.fabs(1 - (points[i][0] * points[i][0]) - (points[i][1] * points[i][1])))
            else:
                points[i][2] = -1 * math.sqrt(
                    math.fabs(1 - (points[i][0] * points[i][0]) - (points[i][1] * points[i][1])))
        else:
            points[i][0] = 1024 * random.random() / RAND_MAX
            points[i][1] = 1024 * random.random() / RAND_MAX
            if i % 2 == 0:
                points[i][2] = 1024 * random.random() / RAND_MAX
            else:
                points[i][2] = -1 * (points[i][0] + points[i][1])
    else:
        points[i][0] = 1024 * random.random() / RAND_MAX
        points[i][1] = 1024 * random.random() / RAND_MAX
        if i % 2 == 0:
            points[i][2] = 1024 * random.random() / RAND_MAX
        else:
            points[i][2] = -1 * (points[i][0] + points[i][1])

cloud.from_array(points)

model_s = pcl.SampleConsensusModelSphere(cloud)
model_p = pcl.SampleConsensusModelPlane(cloud)
if argc > 1:
    if argvs[1] == "-f":
        ransac = pcl.RandomSampleConsensus(model_p)
        ransac.set_DistanceThreshold(.01)
        ransac.computeModel()
        inliers = ransac.get_Inliers()
    elif argvs[1] == "-sf":
        ransac = pcl.RandomSampleConsensus(model_s)
        ransac.set_DistanceThreshold(.01)
        ransac.computeModel()
        inliers = ransac.get_Inliers()
    else:
        inliers = []
else:
    inliers = []

print(str(len(inliers)))
if len(inliers) != 0:
    finalpoints = np.zeros((len(inliers), 3), dtype=np.float32)

    for i in range(0, len(inliers)):
        finalpoints[i][0] = cloud[inliers[i]][0]
        finalpoints[i][1] = cloud[inliers[i]][1]
        finalpoints[i][2] = cloud[inliers[i]][2]

    final.from_array(finalpoints)

isWindows = True
if isWindows == True:
    if argc > 1:
        if argvs[1] == "-f" or argvs[1] == "-sf":
            viewer = pcl.pcl_visualization.PCLVisualizering('3D Viewer')
            viewer.SetBackgroundColor(0, 0, 0)
            viewer.AddPointCloud(final, b'inliers cloud')
            viewer.SetPointCloudRenderingProperties(pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 3, b'inliers cloud')
            viewer.InitCameraParameters()
        else:
            viewer = pcl.pcl_visualization.PCLVisualizering('3D Viewer')
            viewer.SetBackgroundColor(0, 0, 0)
            viewer.AddPointCloud(cloud, b'sample cloud')
            viewer.SetPointCloudRenderingProperties(pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 3, b'sample cloud')
            viewer.InitCameraParameters()
    else:
        viewer = pcl.pcl_visualization.PCLVisualizering('3D Viewer')
        viewer.SetBackgroundColor(0, 0, 0)
        viewer.AddPointCloud(cloud, b'sample cloud')
        viewer.SetPointCloudRenderingProperties(pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 3, b'sample cloud')
        viewer.InitCameraParameters()

    while viewer.WasStopped() != True:
        viewer.SpinOnce(100)
else:
    pass

Python-pcl 随机采样一致性算法

猜你喜欢