【C++】用Ceres从三维点中拟合三维空间中的圆

news2024/11/16 1:52:25

任务描述

在三维空间中有N个点，需要得到过这N个点的最优圆，需要估计圆的圆心、半径和法向量，本文提供了一种方法和代码示例，利用Ceres进行非线性拟合，在cpp上开发。

圆心为三维，半径一维，法向量三维，总共是7维的拟合参数。三个点确定一个圆，所以需要大于等于3的点数。优化器需要初值，这里由N个点中前三个点解算出的圆心、半径、法向量作为初值。当然前三个点可能带有噪声或者离群点，可以读者可以根据实际需要选择稳定的初始化参考点。
完整代码附在文章末尾

实现过程

定义损失函数结构体

损失函数中需要计算残差，是待拟合点到当前圆最短距离之和，而不是简单的到圆心的距离，且必须考虑圆的法向量。设待拟合点为P，圆心为C，半径为R，残差距离的计算公式为：
在这里插入图片描述
C A → = n ⃗ × C P → × n ⃗ ∣ n ⃗ × C P → × n ⃗ ∣ ⋅ R   A P → = C P → − C A →   r e s i d u a l = ∣ A P → ∣ \overrightarrow{CA}=\frac{\vec n \times \overrightarrow{CP} \times \vec n}{|\vec n \times \overrightarrow{CP} \times \vec n|} \cdot R \\ \ \\ \overrightarrow{AP} = \overrightarrow{CP} - \overrightarrow{CA} \\ \ \\ residual = |\overrightarrow{AP}| CA =∣n ×CP ×n ∣n ×CP ×n ​⋅R AP =CP −CA  residual=∣AP ∣
其中A点是 C P → \overrightarrow{CP} CP 向量在圆平面上的投影与圆的交点，通过叉积运算得到 C A → \overrightarrow{CA} CA 的方向，再归一化后乘以半径得到向量 C A → \overrightarrow{CA} CA 。

这里的损失函数结构是根据ceres::AutoDiffCostFunction模板的要求设计的，调用的示例如下：

// 构造Cost函数，计算当前点到外接圆的距离
// 尖括号中的参数分别是struct, 输出的维度，第n个参数块的维度
ceres::CostFunction *cost_function = 
			 newceres::AutoDiffCostFunction<CircleFittingCost, 1, 3, 3, 1>(
			     new CircleFittingCost(points[i]));

所以需要设计一个结构体CircleFittingCost作为损失函数

// 定义计算点到外接圆的距离的Cost函数
struct CircleFittingCost
{
    // 构造函数，传入当前点的坐标
    CircleFittingCost(const Eigen::Vector3d &_point) : m_point(_point) {}

    // 模板函数，计算当前点到外接圆的距离
    template <typename T>
    bool operator()(const T *const center, const T *const norm_vector,
                    const T *const radius, T *residual) const
    {
        Eigen::Matrix<T, 3, 1> C_P(m_point.x() - center[0],
                                   m_point.y() - center[1],
                                   m_point.z() - center[2]);
        Eigen::Matrix<T, 3, 1> cross_result1;
        cross_result1(0) = norm_vector[1] * C_P(2) - norm_vector[2] * C_P(1);
        cross_result1(1) = norm_vector[2] * C_P(0) - norm_vector[0] * C_P(2);
        cross_result1(2) = norm_vector[0] * C_P(1) - norm_vector[1] * C_P(0);
        Eigen::Matrix<T, 3, 1> cross_result2;
        cross_result2(0) = cross_result1(1) * norm_vector[2] - cross_result1(2) * norm_vector[1];
        cross_result2(1) = cross_result1(2) * norm_vector[0] - cross_result1(0) * norm_vector[2];
        cross_result2(2) = cross_result1(0) * norm_vector[1] - cross_result1(1) * norm_vector[0];
        Eigen::Matrix<T, 3, 1> C_A = cross_result2.normalized() *
                                     *radius;

        // Eigen::Vector3d C_A = norm_vector.cross(C_P)
        //                           .cross(norm_vector)
        //                           .normalized() *
        //                       *radius;
        residual[0] = (C_P - C_A).norm();
        // residual[0] = *radius;

        return true;
    }
    Eigen::Vector3d m_point; // 当前点的坐标
};

C点是圆心，P点是传入的待拟合三维点。这里要注意， problem.AddResidualBlock()函数的后面几个传给CircleFittingCost的参数（圆心半径法向量那些）只接受double[]数组和double指针

double C_data[] = {C.x(), C.y(), C.z()};
double norm_vector_data[] = {norm_vector.x(), norm_vector.y(), norm_vector.z()};
 // 构造Cost函数，计算当前点到外接圆的距离
ceres::CostFunction *cost_function = // 尖括号中的参数分别是struct, 输出的维度，第n个参数块的维度
      new ceres::AutoDiffCostFunction<CircleFittingCost, 1, 3, 3, 1>(
          new CircleFittingCost(points[i]));
  // 将Cost函数添加到优化问题中
  problem.AddResidualBlock(cost_function, NULL, C_data, norm_vector_data, &radius);

而传入损失函数结构体CircleFittingCost之后需要都转换成Eigen::Matrix，Ceres才能进行自动求导运算，所以有了下面这些定义，同时Eigen::Matrix不支持叉积运算，所以只能手动计算叉积。Eigen::Vector3d可以进行叉积运算，但是不满足Ceres自动求导的数据结构要求：

// 这段代码在上面出现过，只是复制下来解释

// 定义向量CP，由圆心C指向待拟合点P
Eigen::Matrix<T, 3, 1> C_P(m_point.x() - center[0],
                                   m_point.y() - center[1],
                                   m_point.z() - center[2]);
Eigen::Matrix<T, 3, 1> cross_result1;
cross_result1(0) = norm_vector[1] * C_P(2) - norm_vector[2] * C_P(1);
cross_result1(1) = norm_vector[2] * C_P(0) - norm_vector[0] * C_P(2);
cross_result1(2) = norm_vector[0] * C_P(1) - norm_vector[1] * C_P(0);
Eigen::Matrix<T, 3, 1> cross_result2;
cross_result2(0) = cross_result1(1) * norm_vector[2] - cross_result1(2) * norm_vector[1];
cross_result2(1) = cross_result1(2) * norm_vector[0] - cross_result1(0) * norm_vector[2];
cross_result2(2) = cross_result1(0) * norm_vector[1] - cross_result1(1) * norm_vector[0];
Eigen::Matrix<T, 3, 1> C_A = cross_result2.normalized() * *radius;

计算初始值

取前三个点直接计算初始的圆心、半径、法向量

Eigen::Vector3d v1 = points[1] - points[0];
Eigen::Vector3d v2 = points[2] - points[0];
Eigen::Vector3d v3 = points[2] - points[1];

double sin_A = v1.cross(v2).norm() / (v1.norm() * v2.norm());
double cos_A = v1.dot(v2) / (v1.norm() * v2.norm());
double sin_2A = 2 * sin_A * cos_A;

double sin_B = v1.cross(v3).norm() / (v1.norm() * v3.norm());
double cos_B = -v1.dot(v3) / (v1.norm() * v3.norm());
double sin_2B = 2 * sin_B * cos_B;

double sin_C = v2.cross(v3).norm() / (v2.norm() * v3.norm());
double cos_C = v2.dot(v3) / (v2.norm() * v3.norm());
double sin_2C = 2 * sin_C * cos_C;

Eigen::Vector3d AC = cos_B / (2 * sin_A * sin_C) * v1 + cos_C / (2 * sin_A * sin_B) * v2; // W为圆心点
C = points[0] + AC;
radius = AC.norm();
norm_vector = v1.cross(v2).normalized();

调用优化器进行非线性优化

优化的流程是：

  1. 定义优化问题
  2. 把所有的点构建成cost_function用AddResidualBlock()添加到优化问题中
  3. 添加优化器设置并调用Solve()函数进行优化
  4. 取出数据
ceres::Problem problem; // 定义优化问题
// 添加观测，即待优化点points
for (size_t i = 0; i < points.size(); ++i)
{
    // 构造Cost函数，计算当前点到外接圆的距离
    ceres::CostFunction *cost_function = // 尖括号中的参数分别是struct, 输出的维度，第n个参数块的维度
        new ceres::AutoDiffCostFunction<CircleFittingCost, 1, 3, 3, 1>(
            new CircleFittingCost(points[i]));
    // 将Cost函数添加到优化问题中
    problem.AddResidualBlock(cost_function, NULL, C_data, norm_vector_data, &radius);
}

ceres::Solver::Options options;               // 定义优化器的配置项
options.max_num_iterations = 100;             // 最大迭代次数
options.linear_solver_type = ceres::DENSE_QR; // 线性求解器类型
options.minimizer_progress_to_stdout = true;  // 输出优化过程到终端

ceres::Solver::Summary summary;            	  // 定义优化结果的汇总信息
ceres::Solve(options, &problem, &summary);    // 使用LM算法求解优化问题

// 取出数据，传入优化器的都是double数组和指针，转变成需要的数据
 center.x() = C_data[0];
 center.y() = C_data[1];
 center.z() = C_data[2];
 std::cout << "center_x:  " << center.x() << std::endl;
 std::cout << "center_y:  " << center.y() << std::endl;
 std::cout << "center_z:  " << center.z() << std::endl;
 std::cout << "radius:  " << radius << std::endl;
 norm_vector.x() = norm_vector_data[0];
 norm_vector.y() = norm_vector_data[1];
 norm_vector.z() = norm_vector_data[2];

完整代码

/**
 * @description: 求解外接圆的曲率半径、法向量和圆心位置
 * @param {vector<Eigen::Vector3d>} &points 采样点的坐标
 * @param {Vector3d} &center 拟合圆心
 * @param {double} &radius 拟合圆半径
 * @param {Vector3d} &norm_vector 拟合圆法向量
 * @return {*}
 */
bool fitCircle(const std::vector<Eigen::Vector3d> &points, Eigen::Vector3d &center,
               double &radius, Eigen::Vector3d &norm_vector)
{
    // 初始化圆心和曲率半径
    Eigen::Vector3d C; // 圆心坐标向量
    if (points.size() < 3)
    {
        std::cout << "采样点的数量小于3，计算失败！！" << std::endl;
        return false;
    }
    else
    {
        // 如果采样点数大于3，用前三个点初始化圆法向量、圆心和半径
        // 1.计算法向量
        Eigen::Vector3d v1 = points[1] - points[0];
        Eigen::Vector3d v2 = points[2] - points[0];
        Eigen::Vector3d v3 = points[2] - points[1];

        double sin_A = v1.cross(v2).norm() / (v1.norm() * v2.norm());
        double cos_A = v1.dot(v2) / (v1.norm() * v2.norm());
        double sin_2A = 2 * sin_A * cos_A;

        double sin_B = v1.cross(v3).norm() / (v1.norm() * v3.norm());
        double cos_B = -v1.dot(v3) / (v1.norm() * v3.norm());
        double sin_2B = 2 * sin_B * cos_B;

        double sin_C = v2.cross(v3).norm() / (v2.norm() * v3.norm());
        double cos_C = v2.dot(v3) / (v2.norm() * v3.norm());
        double sin_2C = 2 * sin_C * cos_C;

        Eigen::Vector3d AC = cos_B / (2 * sin_A * sin_C) * v1 + cos_C / (2 * sin_A * sin_B) * v2; // W为圆心点
        C = points[0] + AC;
        radius = AC.norm();
        norm_vector = v1.cross(v2).normalized();
    }
    double C_data[] = {C.x(), C.y(), C.z()};
    double norm_vector_data[] = {norm_vector.x(), norm_vector.y(), norm_vector.z()};

    ceres::Problem problem; // 定义优化问题
    for (size_t i = 0; i < points.size(); ++i)
    {
        // 构造Cost函数，计算当前点到外接圆的距离
        ceres::CostFunction *cost_function = // 尖括号中的参数分别是struct, 输出的维度，第n个参数块的维度
            new ceres::AutoDiffCostFunction<CircleFittingCost, 1, 3, 3, 1>(
                new CircleFittingCost(points[i]));
        // 将Cost函数添加到优化问题中
        problem.AddResidualBlock(cost_function, NULL, C_data, norm_vector_data, &radius);
    }

    ceres::Solver::Options options;               // 定义优化器的配置项
    options.max_num_iterations = 100;             // 最大迭代次数
    options.linear_solver_type = ceres::DENSE_QR; // 线性求解器类型
    options.minimizer_progress_to_stdout = true;  // 输出优化过程到终端

    ceres::Solver::Summary summary;            // 定义优化结果的汇总信息
    ceres::Solve(options, &problem, &summary); // 使用LM算法求解优化问题

    // 更新圆心和法向量，半径直接用传入的radius进行优化就不用更新了
    center.x() = C_data[0];
    center.y() = C_data[1];
    center.z() = C_data[2];
    std::cout << "center_x:  " << center.x() << std::endl;
    std::cout << "center_y:  " << center.y() << std::endl;
    std::cout << "center_z:  " << center.z() << std::endl;
    std::cout << "radius:  " << radius << std::endl;
    norm_vector.x() = norm_vector_data[0];
    norm_vector.y() = norm_vector_data[1];
    norm_vector.z() = norm_vector_data[2];
    return true;
}

// 定义计算点到外接圆的距离的Cost函数
struct CircleFittingCost
{
    // 构造函数，传入当前点的坐标
    CircleFittingCost(const Eigen::Vector3d &_point) : m_point(_point) {}

    // 模板函数，计算当前点到外接圆的距离
    template <typename T>
    bool operator()(const T *const center, const T *const norm_vector,
                    const T *const radius, T *residual) const
    {
        Eigen::Matrix<T, 3, 1> C_P(m_point.x() - center[0],
                                   m_point.y() - center[1],
                                   m_point.z() - center[2]);
        Eigen::Matrix<T, 3, 1> cross_result1;
        cross_result1(0) = norm_vector[1] * C_P(2) - norm_vector[2] * C_P(1);
        cross_result1(1) = norm_vector[2] * C_P(0) - norm_vector[0] * C_P(2);
        cross_result1(2) = norm_vector[0] * C_P(1) - norm_vector[1] * C_P(0);
        Eigen::Matrix<T, 3, 1> cross_result2;
        cross_result2(0) = cross_result1(1) * norm_vector[2] - cross_result1(2) * norm_vector[1];
        cross_result2(1) = cross_result1(2) * norm_vector[0] - cross_result1(0) * norm_vector[2];
        cross_result2(2) = cross_result1(0) * norm_vector[1] - cross_result1(1) * norm_vector[0];
        Eigen::Matrix<T, 3, 1> C_A = cross_result2.normalized() *
                                     *radius;

        // Eigen::Vector3d C_A = norm_vector.cross(C_P)
        //                           .cross(norm_vector)
        //                           .normalized() *
        //                       *radius;
        residual[0] = (C_P - C_A).norm();
        // residual[0] = *radius;

        return true;
    }
    Eigen::Vector3d m_point; // 当前点的坐标
};

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

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

相关文章

深入刨析容器（四）：深入理解容器镜像

深入刨析容器（四）：深入理解容器镜像

容器通过Namespace和Cgroups将自己与宿主机隔离&#xff0c;那么容器里的进程看到文件系统又是什么样子的呢&#xff1f;容器里的程序应该看到完全独立的文件系统&#xff0c;这样它就可以在自己的容器目录&#xff08;比如 /tmp&#xff09;下进行操作&#xff0c;而完全不会受…
阅读更多...
贝莱德CEO力挺比特币！币圈嘲讽：传统金融从嘲笑到开始入场了！

贝莱德CEO力挺比特币！币圈嘲讽：传统金融从嘲笑到开始入场了！

资产管理巨头贝莱德&#xff08;BlackRock&#xff09;首席执行官Larry Fink公开喊话&#xff0c;希望监管者以民主化方式&#xff0c;来看待现货ETF申请。将与监管积极配合&#xff0c;解除他们对现货比特币ETF的疑虑。 六月中旬&#xff0c;贝莱德向美国证券交易委员会&#…
阅读更多...
vue3中Cron表达式的使用

vue3中Cron表达式的使用

效果&#xff1a; <a-form-item label"Cron表达式" name"cron" required><a-input v-show"false" v-model:value"setForm.cron"></a-input><a-button type"primary" size"small" click"…
阅读更多...
使用NVCleanstall导致显卡功率被锁至115W问题解决

使用NVCleanstall导致显卡功率被锁至115W问题解决

以拯救者Y9000K为例&#xff0c;显卡功耗最大可以达到165W&#xff0c;但最近更新至最新的显卡驱动后&#xff0c;发现显卡功率被限制到了115W。一度怀疑是老黄做了手脚。 经过一系列测试后发现&#xff0c;是自己操作姿势不对。 NVIDIA Platform Controllers and Framework这…
阅读更多...
leetcode极速复习版-第四章字符串

leetcode极速复习版-第四章字符串

目录 344. 反转字符串 541. 反转字符串II 剑指Offer 05.替换空格 151.翻转字符串里的单词 剑指Offer58-II.左旋转字符串 28.实现 strStr() 459.重复的子字符串 字符串总结 344. 反转字符串 编写一个函数&#xff0c;其作用是将输入的字符串反转过来。输入字符串以字符数组 char…
阅读更多...
JAVA jfreechart生成柱状图

JAVA jfreechart生成柱状图

JAVA jfreechart生成柱状图 在项目资源评估中&#xff0c;也就是生成word文档里需要根据数据生成柱状图&#xff0c;在网上找到了jfreechart工具包&#xff0c;来生成柱状图&#xff0c;当然他不仅仅只能生成柱状图&#xff0c;还支持折线图、饼状图等等… 过程 导入依赖 &l…
阅读更多...
快速创建剪映草稿

快速创建剪映草稿

实现原理 : JianYingPro 项目文件是 json 的形式存储的，只需要创建draft_content.json,draft_mate_info.json 打开软件后会自动补全。添加一个媒体到轨道顺序 草稿媒体库 -> 内容媒体库-> 轨道片段add_media_to_track 会识别媒体类型，加入到对应轨道。当没有视频轨道时…
阅读更多...
哈希表 基础理论

哈希表 基础理论

什么是哈希表&#xff1f; 哈希表英文名hash table&#xff0c;国内有一些书籍也翻译为散列表。哈希表是根据关键码的值而直接进行访问的数据结构。 直白来讲&#xff0c;其实数组就是一张哈希表&#xff0c;哈希表中关键码就是数组的索引下标&#xff0c;然后通过下标直接访…
阅读更多...
华为云编译构建CodeArts Build新手操作指南

华为云编译构建CodeArts Build新手操作指南

华为云编译构建&#xff08;CodeArts Build&#xff09;基于云端大规模并发加速&#xff0c;为客户提供高速、低成本、配置简单的混合语言构建能力&#xff0c;帮助客户缩短构建时间&#xff0c;提升构建效率。 本文将给各位开发者带来华为云CodeArts Pipeline的手把手初级教学…
阅读更多...
亚马逊买家账号被封的原因

亚马逊买家账号被封的原因

亚马逊封号原因有很多种情况&#xff0c;以下是一些可能导致账号被封的常见原因&#xff1a; 1、违反亚马逊的服务条款&#xff1a;亚马逊有一系列的服务条款和规定&#xff0c;如果您违反了这些规定&#xff0c;比如多次提交虚假评价、涉及欺诈行为、滥用退货政策等&#xff…
阅读更多...
【深度学习】日常笔记9

【深度学习】日常笔记9

泛化误差&#xff08;generalization error&#xff09;是指&#xff0c;模型应⽤在同样从原始样本的分布中 抽取的⽆限多数据样本时&#xff0c;模型误差的期望。考虑对掷硬币的结果&#xff08;类别0&#xff1a;正⾯&#xff0c;类别1&#xff1a;反⾯&#xff09;进⾏分类的…
阅读更多...
AIGC - Stable Diffusion 图像控制插件 ControlNet (OpenPose) 配置与使用

AIGC - Stable Diffusion 图像控制插件 ControlNet (OpenPose) 配置与使用

欢迎关注我的CSDN&#xff1a;https://spike.blog.csdn.net/ 本文地址&#xff1a;https://spike.blog.csdn.net/article/details/131591887 论文&#xff1a;Adding Conditional Control to Text-to-Image Diffusion Models ControlNet 是神经网络结构&#xff0c;用于控制预…
阅读更多...
CentOS7安装详细安装

CentOS7安装详细安装

CentOS 7镜像下载 官网下载链接&#xff1a;http://isoredirect.centos.org/centos/7/isos/x86_64/ step1: 进入下载页&#xff0c;选择阿里云站点进行下载 Actual Country 国内资源 Nearby Countries 周边国家资源 阿里云站点&#xff1a;http://mirrors.aliyun.com/cento…
阅读更多...
开源微服务框架是什么？看完这篇文章就知道了

开源微服务框架是什么？看完这篇文章就知道了

随着低代码开发平台的快速发展&#xff0c;企业实现流程化管理的愿望指日可待。开源微服务框架是什么&#xff1f;都有哪些特点和优势&#xff1f;作为企业&#xff0c;想要提高办公协作效率&#xff0c;做好数据管理&#xff0c;应用专用的开发平台可以少走弯路&#xff0c;创…
阅读更多...
【电子量产工具】6. 业务系统

【电子量产工具】6. 业务系统

文章目录 前言一、业务系统分析二、处理配置文件三、生成界面四、根据输入事件找到按钮五、业务系统总流程测试测试效果&#xff1a;总结 前言 最近看了 电子量产工具 这个项目&#xff0c;本专栏是对该项目的一个总结。 一、业务系统分析 前面实现了各个子系统&#xff1a;显…
阅读更多...
【Java项目】Vue+ElementUI+Ceph实现多类型文件上传功能并实现文件预览功能

【Java项目】Vue+ElementUI+Ceph实现多类型文件上传功能并实现文件预览功能

文章目录 效果演示前端后端Java 效果演示 先说一下我们的需求&#xff0c;我们的需求就是文件上传&#xff0c;之前的接口是只支持上传图片的&#xff0c;之后需求是需要支持上传pdf&#xff0c;所以我就得换接口&#xff0c;把原先图片上传的接口换为后端ceph&#xff0c;但是…
阅读更多...
MV-Map论文研读

MV-Map论文研读

MV-Map MV-Map: Offboard HD-Map Generation with Multi-view Consistency 论文&#xff1a;https://arxiv.org/pdf/2305.08851.pdf code&#xff1a;https://github.com/ZiYang-xie/MV-Map 代码未开源 总体网络结构 简述 论文首次提出以非车载的方式产生高精度地图。可以…
阅读更多...
基于QT使用7z压缩与解压总结

基于QT使用7z压缩与解压总结

1. 概述 本文主要讲述使用7z第三方工具对文件或文件夹进行加密压缩和解密解压相关方法。7z的全称7-Zip&#xff0c;是一款开源软件。&#xff08;资源主页&#xff1a;https://7-zip.org/&#xff09;2. 设计原理 本文主要使用7z.exe通过命令行来实现压缩与解压功能&…
阅读更多...
数据库之MySQL字符集与数据库操作

数据库之MySQL字符集与数据库操作

目录 字符集 CHRARCTER SET 与COLLATION的关联 CHRARCTER SET 定义 基础操作 查看当前MySQL Server支持的 CHARACTER SET 查看特定字符集信息&#xff08;主要包含默认的COLLATION 与 MAXLEN&#xff09; COLLATION 定义 COLLATION后缀 基础操作 查看MySQL Server支持的…
阅读更多...
C++教程（一）开发环境visual studio的安装——图文详细

C++教程（一）开发环境visual studio的安装——图文详细

一、visual studio下载地址&#xff1a; 1、百度网盘 链接&#xff1a;https://pan.baidu.com/s/1QJosSoAT7EumuvyjtC_1Iw?pwdwuqz 提取码&#xff1a;wuqz 2、官网下载 Visual Studio: 面向软件开发人员和 Teams 的 IDE 和代码编辑器 (microsoft.com)https://visualstudio.…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023