Ransac拟合平面（C++编写实现＜不使用任何库＞）

Ransac拟合平面

参考链接：链接1、链接2、链接3 、最小二乘 、链接4 、一种新方法

项目背景简介：考虑到实际项目识别平面上的非编码点，非编码点的重建精度较高，基本在一个平面上，并不希望使用任何库（传统方式基于最小二乘的SVD最小特征向量），故采用本文方式。拍摄环境中存在杂点，在此方式基础上加入Ransac。此种方式也有本身的局限性。

局限性：此方法将最小化垂直于主轴的残差的平方，而不是垂直于平面的残差的平方。如果残差很小（即，所有点都靠近结果平面），则此方法可能就足够了。但是，如果您的观点更加分散，则此方法可能不是最合适的方法。

1、最小二乘优化目标方程（此方法主要在这进行简单操作）

ax + by + cz = -d ---> ax + by + d = -z;

• 方法一：传统方法参考我的另一篇博客优化d（最小化垂直于平面残差平方）

思路总结：

1. 计算点的质心
2. 计算相对于质心的点的协方差矩阵
3. 计算协方差矩阵的最小特征向量。这是平面法线。

• 方法二：此方法将最小化垂直于平面的残差的平方转变为最小化垂直于主轴的残差的平方。
  选择性优化x、y、z其中主轴，选择最大绝对值的平面法线分量进行优化。

思路总结：

1. 计算点的质心
2. 计算相对于质心的点 的协方差矩阵
3. 找到主轴（具有最大绝对值的平面法线分量）并沿该轴进行线性回归

• 方法三：此文为改进方法：X，Y和Z轴进行线性回归

思路总结：

1. 计算点的质心
2. 计算相对于质心的点的协方差矩阵
3. 沿X，Y和Z轴进行线性回归
4. 基于行列式平方的线性回归结果的权重

2、C++编写

#pragma once
#include 
#include 
#include 
#include 


struct float3
{
    float x, y, z;
};

struct plane
{
    float x, y , z, D;
};

inline void ImportPointCloud(char& filename, std::vector<float3> &points)
{
    float3 temp;
    bool mStatus = false;
    std::ifstream import;
    import.open(&filename);
    while (import.peek() != EOF)
    {
        import >> temp.x >> temp.y >> temp.z;
        points.push_back(temp);
    }
    import.close();
}

inline float DistanceToPlane(float3& point, plane& mPlane)
{
     return (mPlane.x * point.x + mPlane.y * point.y + mPlane.z * point.z + mPlane.D);
}

inline void PlaneFromPoints(std::vector<float3> &points, plane &mPlane)
{
    if (points.size() < 3) throw std::runtime_error("Not enough points to calculate plane");

    float3 sum = { 0,0,0 };
 
    for (int i = 0; i < static_cast<int>(points.size()); i++)
    {
        sum.x += points[i].x;
        sum.y += points[i].y;
        sum.z += points[i].z;
    }

    float3 centroid = { 0,0,0 };
    centroid.x = sum.x / float(points.size());
    centroid.y = sum.y / float(points.size());
    centroid.z = sum.z / float(points.size());
    

    float xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
    for (int i = 0; i < static_cast<int>(points.size()); i++)
    {
        float3 temp;
        temp.x = points[i].x - centroid.x;
        temp.y = points[i].y - centroid.y;
        temp.z = points[i].z - centroid.z;
   
        xx += temp.x * temp.x;
        xy += temp.x * temp.y;
        xz += temp.x * temp.z;
        yy += temp.y * temp.y;
        yz += temp.y * temp.z;
        zz += temp.z * temp.z;
    }

    float detX = yy * zz - yz * yz;
    float detY = xx * zz - xz * xz;
    float detZ = xx * yy - xy * xy;

    float detMax = std::max(std::max(detX, detY), detZ);
    if (detMax <= 0)
    {mPlane.x = 0; mPlane.y = 0; mPlane.z = 0; mPlane.D = 0;}

    float3 dir{};
    if (detMax == detX)
    {
        float a = static_cast<float>((xz * yz - xy * zz) / detX);
        float b = static_cast<float>((xy * yz - xz * yy) / detX);
        dir = { 1.0, a, b };
    }
    else if (detMax == detY)
    {
        float a = static_cast<float>((yz * xz - xy * zz) / detY);
        float b = static_cast<float>((xy * xz - yz * xx) / detY);
        dir = { a, 1.0, b };
    }
    else
    {
        float a = static_cast<float>((yz * xy - xz * yy) / detZ);
        float b = static_cast<float>((xz * xy - yz * xx) / detZ);
        dir = { a, b, 1.0 };
    }

    float dirTemp = sqrt(dir.x * dir.x + dir.y * dir.y + dir.z * dir.z);
    dir.x /= dirTemp;
    dir.y /= dirTemp;
    dir.z /= dirTemp;

    mPlane.x = dir.x;
    mPlane.y = dir.y;
    mPlane.z = dir.z;
    mPlane.D = -(mPlane.x * centroid.x + mPlane.y * centroid.y + mPlane.z * centroid.z);
}

void Ransac3DPlane(std::vector<float3>& inPoints, plane& outPlane)
{
    float minSamplePercent = 0.4f;  // Select the initial sample percentage
    int minSampleNum = static_cast<int>(float(inPoints.size())* minSamplePercent); 
    float maxSamplePercent = 0.9f;  // Maximum percentage control iteration stop
    int maxSampleNum = static_cast<int>(float(inPoints.size()) * maxSamplePercent); 

    float distThreshold = 0.2f;
    int iterations = 0;
    int maxIterations = 30;
    std::vector<float3> maxPlanePoints; // Satisfy the maximum number of interior points of the plane model

    while (iterations < maxIterations || maxPlanePoints.size() < maxSampleNum)
    {
        iterations++;
        float3 ptTemp1, ptTemp2, ptTemp3;
        plane planeParame = { 0, 0, 0, 0 };
        std::vector<float3> points;
        std::vector<float3> tempPlanePoints;
        std::random_shuffle(inPoints.begin(), inPoints.end());
        for (int i = 0; i < minSampleNum; i++)
        {
            ptTemp1.x = inPoints[i].x;
            ptTemp1.y = inPoints[i].y;
            ptTemp1.z = inPoints[i].z;

            points.push_back(ptTemp1);
        }

        PlaneFromPoints(points, planeParame);
        points.clear();

        for (int i = 0; i < static_cast<int>(inPoints.size()); i++)
        {
            float distTemp;
            ptTemp2.x = inPoints[i].x;
            ptTemp2.y = inPoints[i].y;
            ptTemp2.z = inPoints[i].z;
            distTemp = DistanceToPlane(ptTemp2, planeParame);
            if (distTemp < distThreshold)
            {
                tempPlanePoints.push_back(ptTemp2);
            }
            else { continue; }
        }
        
        if (tempPlanePoints.size() > maxPlanePoints.size())
        {
            maxPlanePoints.clear();
            maxPlanePoints.reserve(tempPlanePoints.size());
            for (int i = 0; i < static_cast<int>(tempPlanePoints.size()); i++)
            {
                ptTemp3.x = tempPlanePoints[i].x;
                ptTemp3.y = tempPlanePoints[i].y;
                ptTemp3.z = tempPlanePoints[i].z;
                maxPlanePoints.push_back(ptTemp3);
            }
        }
        tempPlanePoints.clear();
        
    }

    PlaneFromPoints(maxPlanePoints, outPlane);
}

void test()
{
    char filename[255];
    sprintf_s(filename, 255, "111.asc");
    std::vector<float3> points;
    plane mPlane;
    ImportPointCloud(*filename, points);
    //PlaneFromPoints(points, mPlane);
    Ransac3DPlane(points, mPlane);
    std::cout << mPlane.x << "  " << mPlane.y << "  " << mPlane.z << "  " << mPlane.D << std::endl;

}

int main()
{
    test();
    return 0;
}

3、测试

原图 程序测试结果 Gemogic测试效果