高精度并行2D圆弧拟合（C++)

news2025/4/25 11:37:33

依赖库

Eigen3 + GLM + Ceres-2.1.0 + glog-0.6.0 + gflag-2.2.2

基本思路

Step 1： RANSAC找到圆弧，保留inliers点；

Step 2：使用ceres非线性优化的方法，拟合inliers点，得到圆心和半径；

-------------------------------------------------

PCL拟合3D圆弧的代码参见    PCL拟合空间3D圆周 fit3DCircle

PCL库拟合圆弧的问题：PCL中坐标值默认使用float类型，在某些高精度场景中不适用。

代码

pcl2d_circle.h

// 2D圆类
#include<common.h>
#include<Eigen/Dense>
#include<mutex>

namespace pcl2d
{
    PCL2D_API class Circle2D {
    public:
        Point2d center = Point2d(0.0);
        double radius = 0.0;

        Circle2D()
        {
            center = Point2d(0.0);
            radius = 0.0;
        }

        Circle2D(const Point2d& c, double r) : center(c), radius(r) {}

        // 计算点到平面的距离
        double distanceTo(const Point2d& p) const {
            return std::abs(glm::distance(p, center) - radius);
        }

        friend std::ostream& operator<<(std::ostream& os, const Circle2D& circle) {
            os << "Circle2D(center: " << circle.center.x << ", " << circle.center.y
                << ", radius: " << circle.radius << ")";
            return os;
        }
    };

    struct Circle2DModel
    {
        //double a, b, c, d;
        Circle2D param;

        std::vector<Point2d> inliers;

        // 从三个点计算平面方程
        bool computeFrom3Points(const Point2d& p1, const Point2d& p2, const Point2d& p3);

        Circle2D fitCircleAlgebraic(const std::vector<Point2d>& points);

        // 使用最小二乘法从内点拟合平面
        double refineWithLeastSquares();

        // 计算点到2D圆的距离
        double distanceTo(const Point2d& p) const {
            return param.distanceTo(p);
        }

        // 评估模型，收集内点
        int evaluate(const std::vector<Point2d>& points, double distanceThreshold);
        
    };


    // 输入原始二维点和圆模型，计算拟合圆的RMSE
    PCL2D_API double calcRMSE(std::vector<Point2d>& points, Circle2D circle);

    // 并行RANSAC平面拟合
    // inlierRatioThresh=0.9 表示内点占比超过90%，可以提前终止迭代
    PCL2D_API Circle2DModel parallelRansacFitCircle2D(const std::vector<Point2d>& points,
        int maxIterations = 1000,
        double distanceThreshold = 0.01,
        int minInliers = 0,
        int numThreads = 4,
        double inlierRatioThresh = 0.8);

    // 生成带噪声的圆弧
    PCL2D_API std::vector<Point2d> generateNoisyArc2D(
        Point2d center,
        double radius,
        double startAngle, double endAngle,
        int numPoints,
        double noiseLevel);

}

pcl2d_circle.cpp

#include"pcl2d_circle.h"
#include<thread>
#include<random>

using namespace pcl2d;

double pcl2d::calcRMSE(std::vector<Point2d>& points, Circle2D circle)
{
    double sum = 0., dx, dy;

    int n = points.size();
    for (int i = 0; i < n; i++)
    {
        dx = points[i].x - circle.center.x;
        dy = points[i].y - circle.center.y;
        sum += SQR(sqrt(dx * dx + dy * dy) - circle.radius);
    }
    return sqrt(sum / (double)n);
}



// 从三个点计算平面方程
bool Circle2DModel::computeFrom3Points(const Point2d& p1, const Point2d& p2, const Point2d& p3) {
    double A = p2.x - p1.x;
    double B = p2.y - p1.y;
    double C = p3.x - p1.x;
    double D = p3.y - p1.y;

    double E = A * (p1.x + p2.x) + B * (p1.y + p2.y);
    double F = C * (p1.x + p3.x) + D * (p1.y + p3.y);

    double G = 2.0 * (A * (p3.y - p2.y) - B * (p3.x - p2.x));

    if (G == 0) { // 如果三点共线，则无法形成圆
        return false;
    }

    param.center.x = (D * E - B * F) / G;
    param.center.y = (A * F - C * E) / G;
    param.radius = std::sqrt(
        std::pow(param.center.x - p1.x, 2) +
        std::pow(param.center.y - p1.y, 2)
    );
    return true;
}

Circle2D Circle2DModel::fitCircleAlgebraic(const std::vector<Point2d>& points) 
{
    int n = points.size();
    if (n < 3) throw std::runtime_error("至少需要3个点来拟合圆");

    Eigen::MatrixXd A(n, 3);
    Eigen::VectorXd b(n);

    for (int i = 0; i < n; ++i) {
        double x = points[i].x, y = points[i].y;
        A(i, 0) = x;
        A(i, 1) = y;
        A(i, 2) = 1.0;
        b(i) = -(x * x + y * y);
    }

    // 解线性方程组 ATAx = ATb
    Eigen::Vector3d solution = (A.transpose() * A).ldlt().solve(A.transpose() * b);

    double a = solution(0);
    double b_val = solution(1);
    double c = solution(2);

    Circle2D circle;
    circle.center.x = -a / 2.0;
    circle.center.y = -b_val / 2.0;
    circle.radius = std::sqrt(a * a + b_val * b_val - 4.0 * c) / 2.0;

    return circle;
}

int Circle2DModel::evaluate(const std::vector<Point2d>& points, double distanceThreshold)
{
    inliers.clear();
    for (const auto& p : points) {
        if (distanceTo(p) < distanceThreshold) {
            inliers.push_back(p);
        }
    }
    return inliers.size();
}

#include<ceres/ceres.h>

struct CircleCostFunctor {
    CircleCostFunctor(double x, double y) : x_(x), y_(y) {}
    template <typename T>
    bool operator()(const T* const center, const T* const radius, T* residual) const {
        residual[0] = ceres::sqrt(
            ceres::pow(x_ - center[0], 2) + ceres::pow(y_ - center[1], 2)
        ) - radius[0];
        return true;
    }
private:
    double x_, y_;
};

bool refineCircleWithCeres(const std::vector<Point2d>& points,
    Point2d& center, double& radius) {
    ceres::Problem problem;
    double center_ceres[2] = { center.x, center.y };
    double radius_ceres = radius;

    for (const auto& p : points) {
        ceres::CostFunction* cost_function =
            new ceres::AutoDiffCostFunction<CircleCostFunctor, 1, 2, 1>(
                new CircleCostFunctor(p.x, p.y));
        problem.AddResidualBlock(cost_function, nullptr, center_ceres, &radius_ceres);
    }

    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    center.x = center_ceres[0];
    center.y = center_ceres[1];
    radius = radius_ceres;

    return summary.IsSolutionUsable();
}

// 使用最小二乘法从内点拟合平面
double Circle2DModel::refineWithLeastSquares()
{
    int N = inliers.size();
    if (N < 3) {
        std::cerr << "At least 3 points required for circle fitting" << std::endl;
        return 0.0;
    }

    refineCircleWithCeres(inliers, param.center, param.radius);

    double err = 0.0;
    double e;
    double r2 = param.radius * param.radius;
    for (int pId = 0; pId < N; ++pId)
    {
        auto v = inliers[pId] - param.center;
        e = glm::dot(v, v) - r2;
        if (e > err) {
            err = e;
        }
    }
    return err;
}

// 并行RANSAC工作函数
void ransacWorkerFitCircle2D(
    const std::vector<Point2d>& points,
    int maxIterations,
    double distanceThreshold,
    int minInliers,
    std::atomic<int>& bestInliers,
    Circle2DModel& bestModel,
    std::mutex& modelMutex,
    std::atomic<bool>& stopFlag,
    double inlierRatioThresh,
    int threadId)
{

    std::random_device rd;
    std::mt19937 gen(rd() + threadId); // 每个线程有不同的种子
    std::uniform_int_distribution<> dis(0, points.size() - 1);

    Circle2DModel localBestModel;
    int localBestInliers = -1;

    for (int i = 0; i < maxIterations && !stopFlag; ++i) {
        // 随机选择3个不重复的点
        int idx1 = dis(gen);
        int idx2, idx3;
        do { idx2 = dis(gen); } while (idx2 == idx1);
        do { idx3 = dis(gen); } while (idx3 == idx1 || idx3 == idx2);

        // 计算2D圆模型
        Circle2DModel model;
        if (!model.computeFrom3Points(points[idx1], points[idx2], points[idx3]))
            continue;

        // 评估模型
        int inliers = model.evaluate(points, distanceThreshold);

        // 更新本地最佳模型
        if (inliers > localBestInliers && inliers >= minInliers) {
            localBestInliers = inliers;
            localBestModel = model;

            // 检查是否需要更新全局最佳模型
            if (localBestInliers > bestInliers) {
                std::lock_guard<std::mutex> lock(modelMutex);
                if (localBestInliers > bestInliers) {
                    bestInliers = localBestInliers;
                    bestModel = localBestModel;

                    // 动态调整: 如果找到足够好的模型，可以提前停止
                    double inlierRatio = static_cast<double>(bestInliers) / points.size();
                    if (inlierRatio > inlierRatioThresh) { // 如果内点比例超过80%，提前停止
                        stopFlag = true;
                    }
                }
            }
        }
    }
}


// 并行RANSAC平面拟合
// inlierRatioThresh=0.9 表示内点占比超过90%，可以提前终止迭代
Circle2DModel pcl2d::parallelRansacFitCircle2D(const std::vector<Point2d>& points,
    int maxIterations/* = 1000*/,
    double distanceThreshold/* = 0.01*/,
    int minInliers/* = 0*/,
    int numThreads/* = 4*/,
    double inlierRatioThresh/* = 0.8*/)
{
    if (points.size() < 3) {
        throw std::runtime_error("At least 3 points are required to fit a plane");
    }

    if (minInliers <= 0) {
        minInliers = static_cast<int>(points.size() * 0.3); // 默认最小内点数为30%
    }

    std::atomic<int> bestInliers(-1);
    Circle2DModel bestModel;
    std::mutex modelMutex;
    std::atomic<bool> stopFlag(false);

    // 计算每个线程的迭代次数
    int iterationsPerThread = maxIterations / numThreads;

    std::vector<std::thread> threads;
    for (int i = 0; i < numThreads; ++i) {
        threads.emplace_back(ransacWorkerFitCircle2D,
            std::ref(points),
            iterationsPerThread,
            distanceThreshold,
            minInliers,
            std::ref(bestInliers),
            std::ref(bestModel),
            std::ref(modelMutex),
            std::ref(stopFlag),
            inlierRatioThresh,
            i
        );
    }

    // 等待所有线程完成
    for (auto& t : threads) {
        t.join();
    }

    // 如果没有找到足够内点的模型，返回第一个模型
    if (bestInliers == -1) {
        bestModel.computeFrom3Points(points[0], points[1], points[2]);
    }

    // 使用最小二乘法优化最佳模型
    bestModel.evaluate(points, distanceThreshold); // 找出所有内点
    bestModel.refineWithLeastSquares();

    return bestModel;
}

std::vector<Point2d> pcl2d::generateNoisyArc2D(
    Point2d center,
    double radius,
    double startAngle, double endAngle,
    int numPoints,
    double noiseLevel)
{

    std::vector<Point2d> points;
    points.reserve(numPoints);

    startAngle = startAngle * glm::pi<double>() / 180.0; // 转换为弧度
    endAngle = endAngle * glm::pi<double>() / 180.0; // 转换为弧度

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> angleDist(startAngle, endAngle);
    std::normal_distribution<> noiseDist(0.0, noiseLevel);

    for (int i = 0; i < numPoints; ++i) {
        double angle = angleDist(gen);
        double x = center.x + radius * std::cos(angle)/* + noiseDist(gen)*/;
        double y = center.y + radius * std::sin(angle);

        points.push_back({ x, y });
    }

    for (int i = 0; i < points.size(); i++)
    {
        points[i] += noiseDist(gen) * glm::normalize(points[i] - center);
    }

    return points;
}

main.cpp

	auto pts = pcl2d::generateNoisyArc2D(pcl2d::Point2d(100, 77), 2.0, 20, 120, 200, 0.1);

	auto model = pcl2d::parallelRansacFitCircle2D(pts, 1000, 0.2, 0, 4, 0.99);

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2342386.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

PostgreSQL 分区表——范围分区SQL实践

PostgreSQL 分区表——范围分区SQL实践

PostgreSQL 分区表——范围分区SQL实践 1、环境准备1-1、新增原始表1-2、执行脚本新增2400w行1-3、创建pg分区表-分区键为创建时间1-4、创建24年所有分区1-5、设置默认分区&#xff08;兜底用&#xff09;1-6、迁移数据1-7、创建分区表索引 2、SQL增删改查测试2-1、查询速度对比…
阅读更多...
SpringCloud 微服务复习笔记

SpringCloud 微服务复习笔记

文章目录 微服务概述单体架构微服务架构 微服务拆分微服务拆分原则拆分实战第一步&#xff1a;创建一个新工程第二步&#xff1a;创建对应模块第三步&#xff1a;引入依赖第四步&#xff1a;被配置文件拷贝过来第五步&#xff1a;把对应的东西全部拷过来第六步&#xff1a;创建…
阅读更多...
【Python爬虫基础篇】--4.Selenium入门详细教程

【Python爬虫基础篇】--4.Selenium入门详细教程

先解释&#xff1a;Selenium&#xff1a;n.硒&#xff1b;硒元素 目录 1.Selenium--简介 2.Selenium--原理 3.Selenium--环境搭建 4.Selenium--简单案例 5.Selenium--定位方式 6.Selenium--常用方法 6.1.控制操作 6.2.鼠标操作 6.3.键盘操作 6.4.获取断言信息 6.5.…
阅读更多...
Langchain检索YouTube字幕

Langchain检索YouTube字幕

创建一个简单搜索引擎&#xff0c;将用户原始问题传递该搜索系统 本文重点&#xff1a;获取保存文档——保存向量数据库——加载向量数据库 专注于youtube的字幕&#xff0c;利用youtube的公开接口&#xff0c;获取元数据 pip install youtube-transscript-api pytube 初始化 …
阅读更多...
【Linux网络】应用层自定义协议与序列化及Socket模拟封装

【Linux网络】应用层自定义协议与序列化及Socket模拟封装

&#x1f4e2;博客主页&#xff1a;https://blog.csdn.net/2301_779549673 &#x1f4e2;博客仓库&#xff1a;https://gitee.com/JohnKingW/linux_test/tree/master/lesson &#x1f4e2;欢迎点赞 &#x1f44d; 收藏 ⭐留言 &#x1f4dd; 如有错误敬请指正&#xff01; &…
阅读更多...
客户案例：西范优选通过日事清实现流程与项目管理的优化

客户案例：西范优选通过日事清实现流程与项目管理的优化

近几年来&#xff0c;新零售行业返璞归真&#xff0c;从线上销售重返线下发展&#xff0c;满足消费者更加多元化的需求&#xff0c;国内家居集合店如井喷式崛起。为在激烈的市场竞争中立于不败之地&#xff0c;西范优选专注于加强管理能力、优化协作效率的“内功修炼”&#xf…
阅读更多...
LabVIEW实现Voronoi图绘制功能

LabVIEW实现Voronoi图绘制功能

该 LabVIEW 虚拟仪器&#xff08;VI&#xff09;借助 MathScript 节点&#xff0c;实现基于手机信号塔位置计算 Voronoi 图的功能。通过操作演示&#xff0c;能直观展示 Voronoi 图在空间划分上的应用。 各部分功能详细说明 随机地形创建部分 功能&#xff1a;根据 “Maximum a…
阅读更多...
爬虫学习——获取动态网页信息

爬虫学习——获取动态网页信息

对于静态网页可以直接研究html网页代码实现内容获取&#xff0c;对于动态网页绝大多数都是页面内容是通过JavaScript脚本动态生成(也就是json数据格式)&#xff0c;而不是静态的&#xff0c;故需要使用一些新方法对其进行内容获取。凡是通过静态方法获取不到的内容&#xff0c;…
阅读更多...
创新项目实训开发日志4

创新项目实训开发日志4

一、开发简介 核心工作内容&#xff1a;logo实现、注册实现、登录实现、上传gitee 工作时间&#xff1a;第十周 二、logo实现 1.设计logo 2.添加logo const logoUrl new URL(/assets/images/logo.png, import.meta.url).href <div class"aside-first">…
阅读更多...
常见接口测试常见面试题（JMeter）

常见接口测试常见面试题（JMeter）

JMeter 是 Apache 提供的开源性能测试工具&#xff0c;主要用于对 Web 应用、REST API、数据库、FTP 等进行性能、负载和功能测试。​它支持多种协议&#xff0c;如 HTTP、HTTPS、JDBC、SOAP、FTP 等。 在一个线程组中&#xff0c;JMeter 的执行顺序通常为&#xff1a;配置元件…
阅读更多...
计算机组成与体系结构：缓存（Cache）

计算机组成与体系结构：缓存（Cache）

目录 为什么需要 Cache&#xff1f; &#x1f9f1; Cache 的分层设计 &#x1f539; Level 1 Cache&#xff08;L1 Cache&#xff09;一级缓存 &#x1f539; Level 2 Cache&#xff08;L2 Cache&#xff09;二级缓存 &#x1f539; Level 3 Cache&#xff08;L3 Cache&am…
阅读更多...
Flutter 在全新 Platform 和 UI 线程合并后，出现了什么大坑和变化？

Flutter 在全新 Platform 和 UI 线程合并后，出现了什么大坑和变化？

Flutter 在全新 Platform 和 UI 线程合并后&#xff0c;出现了什么大坑和变化&#xff1f; 在两个月前&#xff0c;我们就聊过 3.29 上《Platform 和 UI 线程合并》的具体原因和实现方式&#xff0c;而事实上 Platform 和 UI 线程合并&#xff0c;确实为后续原生语言和 Dart 的…
阅读更多...
stm32之GPIO函数详解和上机实验

stm32之GPIO函数详解和上机实验

目录 1.LED和蜂鸣器1.1 LED1.2 蜂鸣器 2.实验2.1 库函数&#xff1a;RCC和GPIO2.1.1 RCC函数1. RCC_AHBPeriphClockCmd2. RCC_APB2PeriphClockCmd3. RCC_APB1PeriphClockCmd 2.1.2 GPIO函数1. GPIO_DeInit2. GPIO_AFIODeInit3. GPIO_Init4. GPIO_StructInit5. GPIO_ReadInputDa…
阅读更多...
用 PyQt5 和 asyncio 打造接口并发测试 GUI 工具

用 PyQt5 和 asyncio 打造接口并发测试 GUI 工具

接口并发测试是测试工程师日常工作中的重要一环&#xff0c;而一个直观的 GUI 工具能有效提升工作效率和体验。本篇文章将带你用 PyQt5 和 asyncio 从零实现一个美观且功能实用的接口并发测试工具。 我们将实现以下功能&#xff1a; 请求方法选择器 添加了一个下拉框 QComboBo…
阅读更多...
Qt实战之将自定义插件(minGW)显示到Qt Creator列表的方法

Qt实战之将自定义插件(minGW)显示到Qt Creator列表的方法

Qt以其强大的跨平台特性和丰富的功能&#xff0c;成为众多开发者构建图形用户界面&#xff08;GUI&#xff09;应用程序的首选框架。而在Qt开发的过程中&#xff0c;自定义插件能够极大地拓展应用程序的功能边界&#xff0c;让开发者实现各种独特的、个性化的交互效果。想象一下…
阅读更多...
【Vue】TypeScript与Vue3集成

【Vue】TypeScript与Vue3集成

个人主页&#xff1a;Guiat 归属专栏&#xff1a;Vue 文章目录 1. 前言2. 环境准备与基础搭建2.1. 安装 Node.js 与 npm/yarn/pnpm2.2. 创建 Vue3 TypeScript 项目2.2.1. 使用 Vue CLI2.2.2. 使用 Vite&#xff08;推荐&#xff09;2.2.3. 目录结构简述 3. Vue3 TS 基础语法整…
阅读更多...
Linux之七大难命令（The Seven Difficult Commands of Linux）

Linux之七大难命令（The Seven Difficult Commands of Linux）

Linux之七大难命令 、背景 作为Linux的初学者&#xff0c;肯定要先掌握高频使用的指令&#xff0c;这样才能让Linux的学习在短时间内事半功倍。但是&#xff0c;有些指令虽然功能强大&#xff0c;但因参数多而让初学者们很害怕&#xff0c;今天介绍Linux中高频使用&#xff0…
阅读更多...
5.3.1 MvvmLight以及CommunityToolkit.Mvvm介绍

5.3.1 MvvmLight以及CommunityToolkit.Mvvm介绍

MvvmLight、CommunityToolkit.Mvvm是开源包，他们为实现 MVVM（Model-View-ViewModel）模式提供了一系列实用的特性和工具，能帮助开发者更高效地构建 WPF、UWP、MAUI 等应用程序。 本文介绍如下： 一、使用(旧)的MvvmLight库 其特点如下，要继承的基类是ViewModelBase；且使用…
阅读更多...
Dbeaver 执行 SQL 语句和执行 SQL 脚本的区别

Dbeaver 执行 SQL 语句和执行 SQL 脚本的区别

执行 SQL 语句 执行 SQL 语句对应图标&#xff1a; 适用于执行单个 SQL 的情形&#xff0c;默认是在光标处或选中的文本上执行 SQL 查询。 实际上同时选择多个 SQL 并通过该方式去执行也可能成功&#xff0c;只是有失败的风险。因此不建议使用它来同时执行多个 SQL 语句。 情况…
阅读更多...
《Python3网络爬虫开发实战（第二版）》配套案例 spa6

《Python3网络爬虫开发实战（第二版）》配套案例 spa6

Scrape | Moviehttps://spa6.scrape.center/ 请求影片列表api时&#xff0c;不仅有分页参数&#xff0c;还多了一个token&#xff0c;通过重发请求发现token有时间限制&#xff0c;所以得逆向token的生成代码。 通过xhr断点定位到接口请求位置 刷新页面或者点翻页按钮&#x…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023