使用 lp_solve 计算 ODR 圆柱拟合（C++ 实现）

huangyhg

目标：
给定一组 3D 点，使用 最小二乘正交距离回归（ODR） 来拟合 最优圆柱参数（圆心、方向向量、半径），并使用 lp_solve 求解 min-max ODR 约束问题。

huangyhg

huangyhg

#include <iostream>
#include <vector>
#include <cmath>
#include "lp_lib.h"  // lp_solve 头文件

struct Point3D {
    double x, y, z;
};

// 计算点 P 到直线 (C, V) 的正交距离
double pointLineDistance(const Point3D& C, const Point3D& V, const Point3D& P) {
    Point3D PC = { P.x - C.x, P.y - C.y, P.z - C.z };
    double dot = PC.x * V.x + PC.y * V.y + PC.z * V.z;
    Point3D proj = { C.x + dot * V.x, C.y + dot * V.y, C.z + dot * V.z };
    double dx = P.x - proj.x, dy = P.y - proj.y, dz = P.z - proj.z;
    return sqrt(dx * dx + dy * dy + dz * dz);
}

// 使用 lp_solve 计算 ODR 圆柱拟合
void fitCylinderODR(const std::vector<Point3D>& points, Point3D& center, Point3D& axis, double& radius) {
    int n = points.size();
    int d = 7;  // 变量：cx, cy, cz, vx, vy, vz, r
    lprec* lp = make_lp(0, d);  // 创建 LP 模型
   
    if (lp == nullptr) {
        std::cerr << "无法创建 lp_solve 模型！" << std::endl;
        return;
    }

    set_verbose(lp, IMPORTANT);  // 仅显示重要信息
    set_minim(lp);  // 目标是最小化

    // 目标函数：Minimize u
    double obj[8] = { 0, 0, 0, 0, 0, 0, 1, 0 };  // 变量 7: u
    set_obj_fn(lp, obj);

    // 约束：-u <= (点到轴的距离 - r) <= u
    for (int i = 0; i < n; ++i) {
        double row[8] = {0};
        Point3D P = points[i];

        row[1] = (P.x - center.x) - ((P.x - center.x) * axis.x) * axis.x;
        row[2] = (P.y - center.y) - ((P.y - center.y) * axis.y) * axis.y;
        row[3] = (P.z - center.z) - ((P.z - center.z) * axis.z) * axis.z;
        row[7] = -1;  // -r

        add_constraint(lp, row, LE, 0);  //  d_i - r <= u
        row[7] = 1;   // +r
        add_constraint(lp, row, GE, 0);  // -d_i + r <= u
    }

    // 约束：方向向量归一化 vx^2 + vy^2 + vz^2 = 1
    double norm_constraint[8] = {0, 0, 0, 1, 1, 1, 0, 0};
    add_constraint(lp, norm_constraint, EQ, 1);

    // 设置变量上下界
    set_bounds(lp, 7, 0, 10);  // 半径 r > 0
    for (int j = 1; j <= 6; j++) {
        set_bounds(lp, j, -10, 10);  // 变量限制在 [-10,10] 之间
    }

    // 运行优化
    if (solve(lp) == OPTIMAL) {
        get_variables(lp, &center.x);
        get_variables(lp, &center.y);
        get_variables(lp, &center.z);
        get_variables(lp, &axis.x);
        get_variables(lp, &axis.y);
        get_variables(lp, &axis.z);
        get_variables(lp, &radius);
        std::cout << "优化成功！拟合圆柱：" << std::endl;
        std::cout << "圆柱中心: (" << center.x << ", " << center.y << ", " << center.z << ")" << std::endl;
        std::cout << "轴方向: (" << axis.x << ", " << axis.y << ", " << axis.z << ")" << std::endl;
        std::cout << "半径: " << radius << std::endl;
    } else {
        std::cerr << "优化失败！" << std::endl;
    }

    delete_lp(lp);
}

int main() {
    std::vector<Point3D> points = {
        {1.0, 0.0, 0.0}, {0.7, 0.7, 1.0}, {0.0, 1.0, 2.0},
        {-0.7, 0.7, 3.0}, {-1.0, 0.0, 4.0}, {-0.7, -0.7, 5.0},
        {0.0, -1.0, 6.0}, {0.7, -0.7, 7.0}
    };
   
    Point3D center = {0, 0, 0};
    Point3D axis = {0, 0, 1};
    double radius = 1.0;

    fitCylinderODR(points, center, axis, radius);
    return 0;
}