ransac拟合平面,代替open3d的segment_plane

0.open3d打包太大了,所以决定网上找找代码

使用open3d拟合平面并且求平面的法向量,open3d打包大概1个g的大小。

python 复制代码
import open3d as o3d
  pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)

    #
    # 使用RANSAC算法拟合平面
    plane_model, inliers = pcd.segment_plane(
        distance_threshold, ransac_n, num_iterations, probability
    )
     plane_normal = np.array(plane_model[:3])
    plane_normal /= np.linalg.norm(plane_normal)
    X_normal = [1, 0, 0]
    Y_normal = [0, 1, 0]
    Z_normal = [0, 0, 1]
    # 计算夹角(单位为弧度)
    angle = np.arccos(np.dot(plane_normal, X_normal))
    # 将夹角转换为角度
    X_angel = degrees(angle)

    # 计算夹角(单位为弧度)
    angle = np.arccos(np.dot(plane_normal, Y_normal))
    # 将夹角转换为角度
    Y_angel = degrees(angle)

    # 计算夹角(单位为弧度)
    angle = np.arccos(np.dot(plane_normal, Z_normal))
    # 将夹角转换为角度
    Z_angel = degrees(angle)

1.找了一个git上的代码

https://github.com/leomariga/pyRANSAC-3D/blob/master/pyransac3d/plane.py

python 复制代码
import random

import numpy as np


class Plane:
    """
    Implementation of planar RANSAC.

    Class for Plane object, which finds the equation of a infinite plane using RANSAC algorithim.

    Call `fit(.)` to randomly take 3 points of pointcloud to verify inliers based on a threshold.

    ![Plane](https://raw.githubusercontent.com/leomariga/pyRANSAC-3D/master/doc/plano.gif "Plane")

    ---
    """

    def __init__(self):
        self.inliers = []
        self.equation = []

    def fit(self, pts, thresh=0.05, minPoints=100, maxIteration=1000):
        """
        Find the best equation for a plane.

        :param pts: 3D point cloud as a `np.array (N,3)`.
        :param thresh: Threshold distance from the plane which is considered inlier.
        :param maxIteration: Number of maximum iteration which RANSAC will loop over.
        :returns:
        - `self.equation`:  Parameters of the plane using Ax+By+Cy+D `np.array (1, 4)`
        - `self.inliers`: points from the dataset considered inliers

        ---
        """
        n_points = pts.shape[0]
        best_eq = []
        best_inliers = []

        for it in range(maxIteration):

            # Samples 3 random points
            id_samples = random.sample(range(0, n_points), 3)
            pt_samples = pts[id_samples]

            # We have to find the plane equation described by those 3 points
            # We find first 2 vectors that are part of this plane
            # A = pt2 - pt1
            # B = pt3 - pt1

            vecA = pt_samples[1, :] - pt_samples[0, :]
            vecB = pt_samples[2, :] - pt_samples[0, :]

            # Now we compute the cross product of vecA and vecB to get vecC which is normal to the plane
            vecC = np.cross(vecA, vecB)

            # The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k
            # We have to use a point to find k
            vecC = vecC / np.linalg.norm(vecC)
            k = -np.sum(np.multiply(vecC, pt_samples[1, :]))
            plane_eq = [vecC[0], vecC[1], vecC[2], k]

            # Distance from a point to a plane
            # https://mathworld.wolfram.com/Point-PlaneDistance.html
            pt_id_inliers = []  # list of inliers ids
            dist_pt = (
                plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + plane_eq[2] * pts[:, 2] + plane_eq[3]
            ) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)

            # Select indexes where distance is biggers than the threshold
            pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]
            if len(pt_id_inliers) > len(best_inliers):
                best_eq = plane_eq
                best_inliers = pt_id_inliers
            self.inliers = best_inliers
            self.equation = best_eq

        return self.equation, self.inliers

2.改进代码

2.1 提速

用的时候发现代码的速度比open3d的慢了50ms左右。找了一圈找到方法了

https://zhuanlan.zhihu.com/p/62238520

就是替换循环次数

python 复制代码
import random

import numpy as np


class Plane:
    """
    Implementation of planar RANSAC.

    Class for Plane object, which finds the equation of a infinite plane using RANSAC algorithim.

    Call `fit(.)` to randomly take 3 points of pointcloud to verify inliers based on a threshold.

    ![Plane](https://raw.githubusercontent.com/leomariga/pyRANSAC-3D/master/doc/plano.gif "Plane")

    ---
    """

    def __init__(self):
        self.inliers = []
        self.equation = []

    def fit(self, pts, thresh=0.05, minPoints=100, maxIteration=1000, P=0.99):
        """
        Find the best equation for a plane.

        :param pts: 3D point cloud as a `np.array (N,3)`.
        :param thresh: Threshold distance from the plane which is considered inlier.
        :param maxIteration: Number of maximum iteration which RANSAC will loop over.
        :param P: desired probability that we get a good sample
        :returns:
        - `self.equation`:  Parameters of the plane using Ax+By+Cy+D `np.array (1, 4)`
        - `self.inliers`: points from the dataset considered inliers

        ---
        """
        n_points = pts.shape[0]
        best_eq = []
        best_inliers = []
        i = 0
        while True:
            if i < maxIteration:
                i += 1
                # Samples 3 random points
                id_samples = random.sample(range(0, n_points), 3)
                pt_samples = pts[id_samples]

                # We have to find the plane equation described by those 3 points
                # We find first 2 vectors that are part of this plane
                # A = pt2 - pt1
                # B = pt3 - pt1

                vecA = pt_samples[1, :] - pt_samples[0, :]
                vecB = pt_samples[2, :] - pt_samples[0, :]

                # Now we compute the cross product of vecA and vecB to get vecC which is normal to the plane
                vecC = np.cross(vecA, vecB)

                # The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k
                # We have to use a point to find k
                vecC = vecC / np.linalg.norm(vecC)
                k = -np.sum(np.multiply(vecC, pt_samples[1, :]))
                plane_eq = [vecC[0], vecC[1], vecC[2], k]

                # Distance from a point to a plane
                # https://mathworld.wolfram.com/Point-PlaneDistance.html
                pt_id_inliers = []  # list of inliers ids
                dist_pt = (
                                  plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + plane_eq[2] * pts[:, 2] +
                                  plane_eq[3]
                          ) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)

                # Select indexes where distance is biggers than the threshold
                pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]
                #https://www.cse.psu.edu/~rtc12/CSE486/lecture15.pdf
                #speed up
                if len(pt_id_inliers) > len(best_inliers):
                    maxIteration = math.log(1 - P) / math.log(1 - pow(len(pt_id_inliers) / n_points, 3))
                    best_eq = plane_eq
                    best_inliers = pt_id_inliers

                self.inliers = best_inliers
                self.equation = best_eq

                if len(pt_id_inliers) > minPoints:
                    break

        return self.equation, self.inliers

2.2 提升精度

经过测试发现,拟合的平面的精度还是比open3d差。然后使用最小二乘法在求一次平面了

python 复制代码
def ransac_fitplan(pts, thresh=5,num_iterations=1000):
    # # 希望的得到正确模型的概率
    n_points = pts.shape[0]
    best_inliers = []
    P = 0.9999
    i=0
    while True:
        if i<num_iterations:
            i+=1
            # 随机在数据中红选出两个点去求解模型
            id_samples = random.sample(range(0, n_points), 3)
            pt_samples = pts[id_samples]
            vecA = pt_samples[1, :] - pt_samples[0, :]
            vecB = pt_samples[2, :] - pt_samples[0, :]

            # Now we compute the cross product of vecA and vecB to get vecC which is normal to the plane
            vecC = np.cross(vecA, vecB)
            # The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k
            # We have to use a point to find k
            vecC = vecC / np.linalg.norm(vecC)
            k = -np.sum(np.multiply(vecC, pt_samples[1, :]))
            plane_eq = [vecC[0], vecC[1], vecC[2], k]

            pt_id_inliers = []  # list of inliers ids
            dist_pt = (
                              plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + plane_eq[2] * pts[:, 2] + plane_eq[3]
                      ) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)

            # Select indexes where distance is biggers than the threshold
            pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]
            if len(pt_id_inliers) > len(best_inliers):
                num_iterations = math.log(1 - P) / math.log(1 - pow(len(pt_id_inliers) / n_points, 3))
                best_inliers = pt_id_inliers
            # 判断是否当前模型已经符合超过一半的点
            if len(pt_id_inliers) > 0.5*n_points:
                break
        else:
            break
    # 最小二乘法拟合平面
    X = np.column_stack((pts[:, :2], np.ones(pts.shape[0])))
    coefficients, _, _, _ = lstsq(X[best_inliers, :], pts[best_inliers, 2])
    return coefficients,best_inliers
相关推荐
haing201910 小时前
已知两个平面点的坐标、切线方向、曲率,使用牛顿迭代法构造三阶 Bézier 曲线的方法
平面·牛顿迭代·三阶bezier
haing20194 天前
两条平面直线之间通过三次多项式曲线进行过渡的方法介绍
平面·g1连续过渡·平面直线
三维重建-光栅投影5 天前
结构光三维重建之线结构光标定(光平面法)
平面
数字孪生家族13 天前
视频孪生技术赋能电力巡检:从“平面监控”到“立体智控”的跨越
平面·变电站三维数字模型·视频孪生电力巡检·视频孪生平台
鲸鱼240114 天前
线性回归笔记
机器学习·平面·线性回归
淡海水14 天前
【URP】[平面阴影]原理与实现
平面·unity·urp·阴影
汤永红15 天前
week4-[二维数组]平面上的点
c++·算法·平面·信睡奥赛
Evand J17 天前
【PSINS工具箱】MATLAB例程,二维平面上的组合导航,EKF融合速度、位置和IMU数据,4维观测量
开发语言·matlab·平面
文火冰糖的硅基工坊1 个月前
[激光原理与应用-261]:理论 - 几何光学 - 平面不过是半径无限大的球面
平面
小一亿1 个月前
【0基础PS】PS工具详解--直接选择工具
学习·平面·adobe·信息可视化·传媒·photoshop