最小二乘法拟合三维直线、三维空间点线距、三维空间直线垂直度、直线度_3d 拟合垂直面

原文

实现XDLine.hpp

#pragma once
#include <vector>
#include <tuple>
#include <algorithm>
#include <cmath>
namespace DDMath
{
    /**
     * @brief 最小二乘法，根据三维坐标拟合出一条直线方程，方程形式(x-x0)/m=(y-y0)/n=z/1，当卡缺点：无法拟合与x0y平行的直线
     *
     * @param points 点坐标std::vector<std::vector<double>>数组,顺序X,Y,Z
     * @return std::tuple<double, double, double, double> {x0,y0,m,n}
     */
    std::tuple<double, double, double, double> Fit3DLine(const std::vector<std::vector<double>> &points)
    {
        double xSum = 0, ySum = 0, zSum = 0, xzSum = 0, yzSum = 0, zzSum = 0;
        for_each(points.begin(), points.end(),
                 [&](const std::vector<double> &point)
                 {
                     xSum += point[0];
                     ySum += point[1];
                     zSum += point[2];
                     xzSum += point[0] * point[2];
                     yzSum += point[1] * point[2];
                     zzSum += point[2] * point[2];
                 });

        double x0 = (zzSum * xSum - xzSum * zSum) * (1 / fabs(3 * zzSum - zSum * zSum)),
               y0 = (zzSum * ySum - yzSum * zSum) * (1 / fabs(3 * zzSum - zSum * zSum)),
               m = (3 * xzSum - xSum * zSum) * (1 / fabs(3 * zzSum - zSum * zSum)),
               n = (3 * yzSum - ySum * zSum) * (1 / fabs(3 * zzSum - zSum * zSum));

        return std::make_tuple(x0, y0, m, n);
    }

    /**
     * @brief 根据点和直线求出直线度、垂直度，直线方程形式(x-x0)/m=(y-y0)/n=z/1
     *
     * @param points 点坐标
     * @return std::tuple<double, double> {垂直度,直线度}
     */
    std::tuple<double, double> VL3DLine(const std::vector<std::vector<double>> &points, double x0, double y0, double m, double n)
    {
        double dv = acos(1 / sqrt(m * m + n * n + 1)) * 57.2956,
               ds = 0.0,
               MAX = 0;

        for_each(points.begin(), points.end(),
                 [&](const std::vector<double> &point)
                 {
                     ds = sqrt(pow(point[0] - x0 - m * point[2], 2) + pow(point[1] - y0 - n * point[2], 2));
                     if (MAX < ds)
                     {
                         MAX = ds;
                     }
                 });
        return std::make_tuple(dv, ds);
    }
    /**
     * @brief 根据点和直线求出点线距离，直线方程形式(x-x0)/m=(y-y0)/n=(z-z0)/p
     *
     * @param point 点坐标
     * @return double 距离
     */
    double PointDist3DLine(const std::vector<double> &point, double x0, double y0, double z0, double m, double n, double p)
    {
        double planeA = m,
               planeB = n,
               planeC = p,
               planeD = point[0] * m + point[1] * n + point[2] * p;
        double t = (planeD - planeA * x0 - planeB * y0 - planeC * z0) / (planeA * m + planeB * n + planeC * p);
        std::vector<double> pointMirror{t * m + x0, t * n + y0, t * p + z0};
        return sqrt(pow(point[0] - pointMirror[0], 2) + pow(point[1] - pointMirror[1], 2) + pow(point[2] - pointMirror[2], 2));
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

使用示例.cpp

#include <vector>
#include <iostream>
#include "XDLine.hpp"
using namespace std;
int main()
{

    vector<vector<double>> points{
        {2, 3, 48}, {4, 5, 50}, {5, 7, 51}};

    auto rs = DDMath::Fit3DLine(points);
    std::cout << std::get<0>(rs)
              << "\t" << std::get<1>(rs)
              << "\t" << std::get<2>(rs)
              << "\t" << std::get<3>(rs)
              << std::endl;
    std::cout << DDMath::PointDist3DLine(std::vector<double>{-6, 1, 21}, -4, -5, -1, 3, 1, 1) << std::endl;
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19