3D点云处理：边缘检测_3 Novel algorithms for 3D surface point cloud boundary detection

1. 基本内容

• 问题：按照论文中连接点的方法，在2D中测试并没有成功，不太确定哪里除了问题，下图为测试结果，并附上测试代码(未整理)
  

2. 代码实现(如果有兴趣可以一起探讨)

#pragma once

#include "AlogLibrary_global.h"

#include 
#include 
#include 
#include 

class ALOGLIBRARY_EXPORT PointCloudBoundary
{
public:
    PointCloudBoundary();

    double calcAngle(const Eigen::Vector2f& vDis)
    {
        float angle = 0;
        //        const float x = std::abs(vDis.x());
        //        const float y = std::abs(vDis.y());

        const float x = vDis.x();
        const float y = vDis.y();
        angle = std::atan2f(y, x);
        return angle;

        if (x >= 0 && y >= 0) {
            angle = 2 * M_PI - std::asin(std::abs(y) / vDis.norm());
        } else if (x > 0 && y < 0) {
            angle = std::asin(std::abs(y) / vDis.norm());
        } else if (x < 0 && y >= 0) {
            angle = M_PI + std::asin(std::abs(y) / vDis.norm());
        } else if (x < 0 && y < 0) {
            angle = M_PI - std::asin(std::abs(y) / vDis.norm());
        }
        //        qDebug() << "angle = " << angle;

        return angle;
    }

    void novelAlgoForBoundary()
    {
        cv::Mat img = cv::Mat::zeros(cv::Size(800, 800), CV_8UC3);

        cv::Point p(400, 400);
        cv::circle(img, p, 3, cv::Scalar(0, 0, 255), -1);

        std::vector<cv::Point> points;
        for (int r = -200; r < 200; r += 40) {
            for (int c = -200; c < 200; c += 40) {
                if (r == 0 && c == 0)
                    continue;
                points.emplace_back(p + cv::Point(c, r));

                cv::circle(img, p + cv::Point(c, r), 3, cv::Scalar(255, 255, 255), -1);
            }
        }

        // max min
        const Eigen::Vector2f oriPoint(p.x, p.y);
        const Eigen::Vector2f xAxis(1, 0);
        const Eigen::Vector2f yAxis(0, -1);
        std::vector<float> dists;
        std::vector<float> angles;
        for (const auto& point : points) {
            const Eigen::Vector2f vPoint(point.x, point.y);
            Eigen::Vector2f vDis = (vPoint - oriPoint);
            const float dist = vDis.norm();
            // const float angle = std::atan2(vDis.dot(yAxis), vDis.dot(xAxis));

            float angle = calcAngle(vDis);
            dists.emplace_back(dist);
            angles.emplace_back(angle);
        }

        std::sort(dists.begin(), dists.end());
        std::sort(angles.begin(), angles.end());

        const float minDist = dists.front();
        const float maxDist = dists.back();
        const float difDist = maxDist - minDist;

        const float minAngle = angles.front();
        const float maxAngle = angles.back();
        const float difAngle = maxAngle - minAngle;

        const float angleBis = 0.5;
        qDebug() << minDist << maxDist << minAngle << maxAngle;

        // find start point
        float minBeta = FLT_MAX;
        float startIndex = 0;
        float normalizedAngle = 0;
        float radAngle = 0;

        for (size_t i = 0; i < points.size(); ++i) {
            const Eigen::Vector2f vPoint(points[i].x, points[i].y);
            Eigen::Vector2f vDis = (vPoint - oriPoint);
            const float dist = vDis.norm();
            //            const float angle = std::atan2(vDis.dot(yAxis), vDis.dot(xAxis));
            float angle = calcAngle(vDis);

            const float normalizedDist = (dist - minDist) / difDist;
            const float normalizedA = (angle - minAngle) / difAngle;
            const float beta = normalizedDist + std::abs(normalizedA - angleBis);
            qDebug() << "beta = " << beta;

            if (beta < minBeta) {
                minBeta = beta;
                startIndex = i;
                normalizedAngle = normalizedA;
                radAngle = angle;
            }
        }
        cv::Point startPoint = points[startIndex];
        cv::circle(img, points[startIndex], 3, cv::Scalar(0, 255, 0), -1);
        points.erase(points.begin() + startIndex);

        std::vector<float> normalizedAngles;
        std::vector<float> radAngles;
        normalizedAngles.emplace_back(normalizedAngle);
        radAngles.emplace_back(radAngle);

        float sumAngle = 0;
        float sumAngleAbs = 0;

        float c = 1;

        std::vector<cv::Point> orderPoints;
        while (true) {
            if (sumAngleAbs > 2 * M_PI) {
                break;
            }

            int nextIndex = 0;
            float nextAngle = 0;
            float nextRadAngle = 0;
            float lastAngle = normalizedAngles.back();
            float secondLastAngle = 0;
            minBeta = FLT_MAX;
            for (size_t i = 0; i < points.size(); ++i) {
                const Eigen::Vector2f vPoint(points[i].x, points[i].y);
                Eigen::Vector2f vDis = (vPoint - oriPoint);
                const float dist = vDis.norm();

                float angle = calcAngle(vDis);
                //  angle = std::atan2(vDis.dot(yAxis), vDis.dot(xAxis));

                const float normalizedDist = (dist - minDist) / difDist;
                const float normalizedA = (angle - minAngle) / difAngle;

                //                qDebug() << "normalizedDist = " << normalizedDist
                //                         << "; normalizedA: " << normalizedA;

                if (normalizedAngles.size() > 1) {
                    lastAngle = normalizedAngles.back();
                    secondLastAngle = normalizedAngles[normalizedAngles.size() - 2];
                    if ((normalizedA - lastAngle) * (lastAngle - secondLastAngle) > 0)
                        c = -1;
                    else
                        c = 1;
                } else {
                    c = 1;
                }

                const float beta = normalizedDist + (normalizedA - lastAngle) * c;
                if (beta < minBeta) {
                    minBeta = beta;
                    nextIndex = i;
                    nextAngle = normalizedA;
                    nextRadAngle = angle;
                }
            }
            qDebug() << "nextAngle = " << nextAngle;
            orderPoints.emplace_back(points[nextIndex]);

            // sumAngle += (nextRadAngle - radAngles.back());
            // sumAngleAbs += std::abs(nextRadAngle - radAngles.back());

            sumAngle += (nextAngle - normalizedAngles.back());
            sumAngleAbs += std::abs(nextAngle - normalizedAngles.back());

            normalizedAngles.emplace_back(nextAngle);
            radAngles.emplace_back(nextRadAngle);
            if (!points.empty())
                points.erase(points.begin() + nextIndex);
            else
                break;
        }

        for (size_t i = 0; i < orderPoints.size(); ++i) {
            std::string text = QString::number(i + 1).toStdString();
            if (i == 0) {
                cv::line(img, startPoint, orderPoints[i], cv::Scalar(255, 0, 0), 1);

            } else {
                cv::line(img, orderPoints[i - 1], orderPoints[i], cv::Scalar(255, 0, 0), 1);
            }
            cv::putText(img, text, orderPoints[i], cv::FONT_HERSHEY_SCRIPT_SIMPLEX, 0.5,
                        cv::Scalar(0, 0, 255));
        }
        cv::imshow("img", img);
    }
};

3. 参考

1. Novel algorithms for 3D surface point cloud boundary detection