opencv--3d数据拟合平面并对倾斜平面矫正

对于深度数据而言，mat记录的是深度值，当对深度值进行各种处理，例如获取直线、圆、椭圆等其他形状时，如果平面没有完全水平，你使用opencv处理精度是有损失的，因此这里使用opencv 先对平面进行矫正，矫正原理是在有效平面内随机采集3000点的深度数据，使用深度数据进行拟合平面，计算平面的倾斜较大，然后使用角度对原始数据进行矫正，代码如下：

// 将深度图转换为点云
std::vector<cv::Point3f> depthToPointCloud(const cv::Mat& depthImage) {
    std::vector<cv::Point3f> pointCloud;

    // 设置随机数生成器
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> distRow(0, depthImage.rows - 1);
    std::uniform_int_distribution<> distCol(0, depthImage.cols - 1);

    // 要提取的像素点数量
    int numPixels = 2000; // 你可以根据需要调整这个数量

    // 存储随机提取的像素点
    std::vector<cv::Vec3b> randomPixels;

    for (int i = 0; i < numPixels; ++i) {
        int y = distRow(gen);
        int x = distCol(gen);
        //randomPixels.push_back(image.at<cv::Vec3b>(row, col));

        cv::Point3f point((float)x, (float)y, depthImage.at<float>(y, x)); // 构造三维点
        pointCloud.push_back(point); // 添加到点云
    }

    //for (int y = 0; y < depthImage.rows; ++y) {
    //    for (int x = 0; x < depthImage.cols; ++x) {
    //        float depth = depthImage.at<float>(y, x); // 获取深度值
    //        //if (std::isnan(depth) || depth <= 0.0) {
    //        //    continue; // 跳过无效深度值
    //        //}
    //        cv::Point3f point((float)x, (float)y, depth); // 构造三维点
    //        pointCloud.push_back(point); // 添加到点云
    //    }
    //}

    return pointCloud;
}

// 函数：拟合平面
cv::Vec4f fitPlaneRANSAC(const std::vector<cv::Point3f>& points, int maxIter = 1000, float threshold = 0.01) {
    if (points.empty())
        return cv::Vec4f();
    cv::Vec4f bestPlane;
    int bestInliers = 0;

    for (int iter = 0; iter < maxIter; ++iter) {
        // 随机选择三个点
        std::vector<int> indices(points.size());
        std::iota(indices.begin(), indices.end(), 0);
        std::shuffle(indices.begin(), indices.end(), std::default_random_engine(5));

        cv::Point3f p1 = points[indices[0]];
        cv::Point3f p2 = points[indices[1]];
        cv::Point3f p3 = points[indices[2]];

        // 计算平面的法向量
        cv::Point3f v1 = p2 - p1;
        cv::Point3f v2 = p3 - p1;
        cv::Point3f normal = v1.cross(v2);
        normal /= cv::norm(normal);

        // 平面公式：Ax + By + Cz + D = 0
        float D = -normal.dot(p1);
        cv::Vec4f plane(normal.x, normal.y, normal.z, D);

        // 计算内点数量
        int inliers = 0;
        for (const auto& point : points) {
            float distance = std::abs(plane[0] * point.x + plane[1] * point.y + plane[2] * point.z + plane[3]);
            if (distance < threshold) {
                inliers++;
            }
        }

        // 更新最佳平面
        if (inliers > bestInliers) {
            bestInliers = inliers;
            bestPlane = plane;
        }
    }

    return bestPlane;
}

// 函数：矫正平面
void correctPlane(const cv::Vec4f& plane, cv::Mat& points) {

    cv::Mat normal_m, normal_m_8;
    cv::normalize(points, normal_m, 1, 0, cv::NORM_MINMAX);
    normal_m.convertTo(normal_m_8, CV_8U, 255.0);

    cv::Vec3f normal(plane[0], plane[1], plane[2]);
    cv::Vec3f zAxis(0, 0, 1);

    cv::Vec3f rotationAxis = normal.cross(zAxis);
    float angle = std::acos(normal.dot(zAxis) / (cv::norm(normal) * cv::norm(zAxis)));

    cv::Mat rotationMatrix;
    cv::Rodrigues(rotationAxis * angle, rotationMatrix);

    for (int i = 0; i < points.rows; ++i) {
        cv::Vec3f point = points.at<cv::Vec3f>(i);
        // 将 point 转换为 cv::Mat 类型
        cv::Mat pointMat = (cv::Mat_<float>(3, 1) << point[0], point[1], point[2]);
        // 矩阵乘法
        cv::Mat transformedPointMat = rotationMatrix * pointMat;
        // 将结果转换回 cv::Vec3f 类型
        points.at<cv::Vec3f>(i) = cv::Vec3f(transformedPointMat.at<float>(0), transformedPointMat.at<float>(1), transformedPointMat.at<float>(2));
    }

    cv::Mat normal_m_, normal_m_8_;
    cv::normalize(points, normal_m_, 1, 0, cv::NORM_MINMAX);
    normal_m_.convertTo(normal_m_8_, CV_8U, 255.0);

}

从矫正前的数据和矫正后的数据可以发现，平面得到了很好得了很好的矫正。