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

任务描述

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

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

实现过程

定义损失函数结构体

损失函数中需要计算残差，是待拟合点到当前圆最短距离之和，而不是简单的到圆心的距离，且必须考虑圆的法向量。设待拟合点为P，圆心为C，半径为R，残差距离的计算公式为：

$$\begin{gathered} \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}| \end{gathered}$$
其中A点是$\overrightarrow{CP}$向量在圆平面上的投影与圆的交点，通过叉积运算得到$\overrightarrow{CA}$的方向，再归一化后乘以半径得到向量$\overrightarrow{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; // 当前点的坐标
};