3D点圆柱拟合

圆柱拟合

namespace cylinder_fitting {
void preprocess(const int n, const Eigen::Matrix<float, 3, -1>& points,
                Eigen::Matrix<float, 3, -1>& X, Eigen::Vector3f& average,
                Eigen::Vector<float, 6>& mu, Eigen::Matrix3f& F0, Eigen::Matrix<float, 3, 6>& F1,
                Eigen::Matrix<float, 6, 6>& F2)
{
    average = points.rowwise().mean();
    X = points.colwise() - average;
    Eigen::Matrix<float, 6, -1> products(6, n);
    products.setZero();
    mu.setZero();
    for (int i = 0; i < n; i++) {
        products(0, i) = X(0, i) * X(0, i);
        products(1, i) = X(0, i) * X(1, i);
        products(2, i) = X(0, i) * X(2, i);
        products(3, i) = X(1, i) * X(1, i);
        products(4, i) = X(1, i) * X(2, i);
        products(5, i) = X(2, i) * X(2, i);

        mu(0) += products(0, i);
        mu(1) += 2 * products(1, i);
        mu(2) += 2 * products(2, i);
        mu(3) += products(3, i);
        mu(4) += 2 * products(4, i);
        mu(5) += products(5, i);
    }
    mu /= n;

    F0.setZero();
    F1.setZero();
    F2.setZero();
    for (int i = 0; i < n; i++) {
        Eigen::Vector<float, 6> delta;
        delta(0) = products(0, i) - mu(0);
        delta(1) = 2 * products(1, i) - mu(1);
        delta(2) = 2 * products(2, i) - mu(2);
        delta(3) = products(3, i) - mu(3);
        delta(4) = 2 * products(4, i) - mu(4);
        delta(5) = products(5, i) - mu(5);

        F0(0, 0) += products(0, i);
        F0(0, 1) += products(1, i);
        F0(0, 2) += products(2, i);
        F0(1, 1) += products(3, i);
        F0(1, 2) += products(4, i);
        F0(2, 2) += products(5, i);

        F1 += X.col(i) * delta.transpose();
        F2 += delta * delta.transpose();
    }

    F0 /= n;
    F0(1, 0) = F0(0, 1);
    F0(2, 0) = F0(0, 2);
    F0(2, 1) = F0(1, 2);

    F1 /= n;
    F2 /= n;
}

float G(const int n, const Eigen::Matrix<float, 3, -1>& X, const Eigen::Vector<float, 6>& mu,
        const Eigen::Matrix3f& F0, const Eigen::Matrix<float, 3, 6>& F1,
        const Eigen::Matrix<float, 6, 6>& F2, Eigen::Vector3f& W, Eigen::Vector3f& PC, float& rSqr)
{
    Eigen::Matrix3f P = Eigen::Matrix3f::Identity() - W * W.transpose();
    Eigen::Matrix3f S;
    S << 0, -W(2), W(1), W(2), 0, -W(0), -W(1), W(0), 0;
    Eigen::Matrix3f A = P * F0 * P;
    Eigen::Matrix3f hatA = -(S * A * S);
    Eigen::Matrix3f hatAA = hatA * A;
    float trace = hatAA.trace();
    Eigen::Matrix3f Q = hatA / trace;
    Eigen::Vector<float, 6> p = {P(0, 0), P(0, 1), P(0, 2), P(1, 1), P(1, 2), P(2, 2)};
    Eigen::Vector3f alpha = F1 * p;
    Eigen::Vector3f beta = Q * alpha;
    float error = (p.dot(F2 * p) - 4 * alpha.dot(beta) + 4 * beta.dot(F0 * beta)) / n;
    PC = beta;
    rSqr = p.dot(mu) + beta.dot(beta);
    return error;
}

float fitCylinder(const int n, const Eigen::Matrix<float, 3, -1>& points, float& rSqr,
                  Eigen::Vector3f& C, Eigen::Vector3f& W)
{
    Eigen::Matrix<float, 3, -1> X(3, n);
    Eigen::Vector<float, 6> mu;
    Eigen::Matrix3f F0;
    Eigen::Matrix<float, 3, 6> F1(3, 6);
    Eigen::Matrix<float, 6, 6> F2(6, 6);

    Eigen::Vector3f average;
    preprocess(n, points, X, average, mu, F0, F1, F2);

    float minError = FLT_MAX;
    W.setZero();
    C.setZero();
    rSqr = 0;
    constexpr int maxj = 5000;
    constexpr int maxi = 7000;
    for (int j = 0; j <= maxj; j++) {
        float phi = std::numbers::pi_v<float> / 2 * j / maxj;
        float csphi = cos(phi), snphi = sin(phi);
        for (int i = 0; i < maxi; i++) {
            float theta = 2 * std::numbers::pi_v<float> * i / maxi;
            float cstheta = cos(theta), sntheta = sin(theta);
            Eigen::Vector3f currentW(cstheta * snphi, sntheta * snphi, csphi);
            Eigen::Vector3f currentC;
            float currentRSqr;
            float error = G(n, X, mu, F0, F1, F2, currentW, currentC, currentRSqr);
            if (error < minError) {
                minError = error;
                W = currentW;
                C = currentC;
                rSqr = currentRSqr;
            }
        }
    }
    C += average;
    return minError;
}

} // namespace cylinder_fitting