请使用 Levenberg-Marquardt 方法求解最小二乘法的矩形

huangyhg

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>  // Levenberg-Marquardt 模块
#include <unsupported/Eigen/NumericalDiff>          // 数值微分支持
#include <cmath>                                    // 用于三角函数

using namespace std;
using namespace Eigen;

// 定义 2D 点结构（使用 Eigen::Vector2d）
struct Point2D
{
    Vector2d position;  // 使用Eigen的向量

    Point2D(double x = 0.0, double y = 0.0)
    {
        position << x, y;
    }
};

// MathHelper 类包含距离计算函数
class MathHelper
{
public:
    // 计算点到矩形边界的距离
    static double GetPointToRectDistance(const Vector2d& point, const Vector2d& center, double width, double height, double theta)
    {
        // 将矩形的旋转应用到点上，将其转回未旋转状态
        double cosTheta = cos(theta);
        double sinTheta = sin(theta);
        Vector2d pointRotated;
        pointRotated(0) = cosTheta * (point(0) - center(0)) + sinTheta * (point(1) - center(1));
        pointRotated(1) = -sinTheta * (point(0) - center(0)) + cosTheta * (point(1) - center(1));

        // 计算点到未旋转矩形的最短距离
        double dx = max(abs(pointRotated(0)) - width / 2.0, 0.0);
        double dy = max(abs(pointRotated(1)) - height / 2.0, 0.0);
        return sqrt(dx * dx + dy * dy);
    }
};

// 定义用于 Levenberg-Marquardt 的 Functor，继承自 DenseFunctor
struct RectangleFitFunctor : Eigen::DenseFunctor<double>
{
    const std::vector<Point2D>& points;  // 数据点集

    // 构造函数
    RectangleFitFunctor(const std::vector<Point2D>& pts)
        : Eigen::DenseFunctor<double>(5, pts.size()), points(pts) {}

    // 计算误差
    int operator()(const VectorXd& params, VectorXd& fvec) const override
    {
        // 矩形参数：中心坐标、宽、高、旋转角
        Vector2d center(params(0), params(1));
        double width = params(2);
        double height = params(3);
        double theta = params(4);

        for (int i = 0; i < points.size(); ++i)
        {
            // 计算每个点到矩形边界的距离
            fvec(i) = MathHelper::GetPointToRectDistance(points[i].position, center, width, height, theta);
        }
        return 0;
    }
};

// FitRectangleToPointCloud 函数，使用 Levenberg-Marquardt 优化矩形
tuple<Vector2d, double, double, double> FitRectangleToPointCloud(const vector<Point2D>& points)
{
    // 定义拟合函数的 Functor
    RectangleFitFunctor functor(points);

    // 初始猜测：点集的质心作为矩形中心，宽和高为点的范围，旋转角度为 0
    Vector2d initialCenter = Vector2d::Zero();
    for (const auto& pt : points)
    {
        initialCenter += pt.position;
    }
    initialCenter /= points.size();  // 质心作为初始矩形中心

    double initialWidth = 1.0;   // 初始宽度（假设）
    double initialHeight = 1.0;  // 初始高度（假设）
    double initialTheta = 0.0;   // 初始旋转角度

    // 构建初始参数向量
    VectorXd initialGuess(5);
    initialGuess << initialCenter(0), initialCenter(1), initialWidth, initialHeight, initialTheta;

    // 使用 Levenberg-Marquardt 优化器
    Eigen::LevenbergMarquardt<RectangleFitFunctor, double> lm(functor);
    lm.parameters.maxfev = 200;  // 设置最大迭代次数
    int info = lm.minimize(initialGuess);  // 执行优化

    // 返回优化后的矩形参数：中心坐标、宽、高、旋转角度
    return make_tuple(Vector2d(initialGuess(0), initialGuess(1)), initialGuess(2), initialGuess(3), initialGuess(4));
}

int main()
{
    // 定义一些测试点
    vector<Point2D> points = {
        Point2D(1, 1),
        Point2D(2, 2),
        Point2D(3, 3),
        Point2D(4, 5),
        Point2D(6, 5),
        Point2D(7, 4)
    };

    // 调用拟合函数
    auto [center, width, height, theta] = FitRectangleToPointCloud(points);

    // 输出结果
    cout << "拟合矩形中心: (" << center(0) << ", " << center(1) << "), 宽度: " << width << ", 高度: " << height << ", 旋转角度: " << theta << " 弧度" << endl;

    return 0;
}

huangyhg

缺点这个拟合随初始值扰动