# 纯python





![在这里插入图片描述](/static/img/images/04097.png)


以下是一个简单的Python示例，演示了如何使用NumPy库实现一个一维卡尔曼滤波器。

```python
import numpy as np
import matplotlib.pyplot as plt
def kalman_filter(initial_state, initial_estimate_error, process_variance, measurement_variance, measurements):
    num_measurements = len(measurements)
    state_estimate = initial_state
    estimate_error = initial_estimate_error
    filtered_state_estimates = []
    for i in range(num_measurements):
        # Prediction step
        predicted_state = state_estimate
        predicted_estimate_error = estimate_error + process_variance
        # Update step
        kalman_gain = predicted_estimate_error / (predicted_estimate_error + measurement_variance)
        state_estimate = predicted_state + kalman_gain * (measurements[i] - predicted_state)
        estimate_error = (1 - kalman_gain) * predicted_estimate_error
        filtered_state_estimates.append(state_estimate)
    return filtered_state_estimates
# Generate some noisy measurements
np.random.seed(0)
true_values = np.linspace(0, 10, num=50)
measurements = true_values + np.random.normal(0, 0.5, size=len(true_values))
# Kalman filter parameters
initial_state = measurements[0]
initial_estimate_error = 1
process_variance = 0.1
measurement_variance = 0.5
# Apply Kalman filter
filtered_estimates = kalman_filter(initial_state, initial_estimate_error, process_variance, measurement_variance, measurements)
# Plot the results
plt.figure(figsize=(10, 6))
plt.plot(true_values, label='True Values', linestyle='dashed')
plt.plot(measurements, label='Noisy Measurements', marker='o', markersize=5)
plt.plot(filtered_estimates, label='Filtered Estimates', linestyle='dotted')
plt.legend()
plt.xlabel('Time Step')
plt.ylabel('Value')
plt.title('Kalman Filter Example')
plt.show()
```

卡尔曼滤波参数的含义如下：

1. `initial_state`: 初始状态估计值，通常设置为第一个测量值。在卡尔曼滤波开始时，您的系统的初始状态估计值。

2. `initial_estimate_error`: 初始状态估计误差，表示初始状态估计值的不确定性。通常设置为一个较大的值，以表示初始状态的不确定性。

3. `process_variance`: 过程方差，表示系统模型中状态转移的噪声或不确定性。较大的值表示系统模型中的不确定性较大。

4. `measurement_variance`: 测量方差，表示传感器测量的噪声或不确定性。较大的值表示测量值的不确定性较大。

在卡尔曼滤波中，这些参数的调整会影响滤波器的性能。例如，如果您预计系统模型比传感器测量更可靠，则可以适当减小`measurement_variance`。如果初始状态估计值的不确定性较大，您可以增加`initial_estimate_error`。调整这些参数可能需要一些试验和经验。

![在这里插入图片描述](/static/img/images/04098.png)

![在这里插入图片描述](/static/img/images/04099.png)





# 基于pykalman 库
```go
import numpy as np
import matplotlib.pyplot as plt
from pykalman import KalmanFilter
# Generate some noisy measurements
np.random.seed(0)
true_values = np.linspace(0, 10, num=50)
measurements = true_values + np.random.normal(0, 0.5, size=len(true_values))
# Create a Kalman Filter object
kf = KalmanFilter(initial_state_mean=measurements[0], n_dim_obs=1)
# Apply Kalman filter
filtered_state_means, _ = kf.filter(measurements)
# Plot the results
plt.figure(figsize=(10, 6))
plt.plot(true_values, label='True Values', linestyle='dashed')
plt.plot(measurements, label='Noisy Measurements', marker='o', markersize=5)
plt.plot(filtered_state_means, label='Filtered Estimates', linestyle='dotted')
plt.legend()
plt.xlabel('Time Step')
plt.ylabel('Value')
plt.title('Kalman Filter Example using pykalman')
plt.show()
```

【卡尔曼滤波】用Python实现卡尔曼滤波效果，Python

# 小波变换





小波变换（Wavelet Transform）是一种数学信号处理技术，用于将信号或图像分解为不同频率的小波成分，从而可以在不同时间尺度上分析信号的特征。小波变换具有许多重要的特性，使其在许多领域中有广泛的应用。

小波变换的意义包括以下几个方面：

1. 多尺度分析：小波变换可以在不同的时间尺度上分析信号，这意味着它可以同时提供关于信号的短时和长时特征信息。这使得小波变换在时频域分析中具有优势，特别是对于非平稳信号的处理。

2. 局部特征提取：小波变换是一种局部分析方法，它可以有效地捕获信号的局部特征，如突变、边缘等。相比之下，傅里叶变换是一种全局分析方法，往往无法提供局部特征信息。

3. 压缩和去噪：小波变换可以将信号分解为多个小波系数，其中包含不同频率的信息。通过适当选择小波系数，可以实现信号的压缩和去噪，去除噪声和冗余信息，从而提高信号处理的效果。

4. 图像处理：小波变换在图像处理中也有重要的应用。它可以用于图像压缩、边缘检测、纹理分析等任务，同时保持图像的局部特征和结构信息。

5. 特征提取：小波变换可以用于特征提取，从信号或图像中提取有用的特征信息，用于分类、识别和模式匹配等任务。

6. 数据分析：小波变换在数据分析中有广泛的应用，包括金融市场分析、生物医学信号处理、地震信号处理等领域。

小波变换是一种强大的数学工具，可以用于多尺度分析、特征提取、信号压缩和去噪等多个领域。它的应用涵盖了信号处理、图像处理、数据分析和模式识别等广泛领域，为我们理解信号的时频特性、提取重要特征和优化数据处理提供了重要手段。


小波变换（Wavelet Transform）可以用数学公式表示为以下形式：

给定一个连续函数或离散信号 $f(t)$，小波变换是通过将原始信号与一组小波基函数进行内积运算得到的。这些小波基函数是原始信号在不同频率和时间尺度上的缩放和平移版本。

连续小波变换（Continuous Wavelet Transform，CWT）的数学公式为：

$$

CWT(a, b) = \int_{-\infty}^{\infty} f(t) \psi^* \left( \frac{t-b}{a} \right) dt

$$

其中，$CWT(a, b)$ 是连续小波变换的结果，$a$ 和 $b$ 是小波基函数的缩放因子和平移因子，$\psi^*(t)$ 是复共轭小波基函数。

复共轭小波基函数是小波变换中的一个重要概念。在小波变换中，我们通常使用两类小波基函数：一个是正向小波基函数（Mother Wavelet），另一个是复共轭小波基函数（Complex Conjugate Wavelet）。

设正向小波基函数为 $\psi(t)$，复共轭小波基函数为 $\psi^*(t)$。复共轭小波基函数是与正向小波基函数相对应的共轭复数函数。

数学上，复共轭小波基函数可以表示为：

$$

\psi^*(t) = \psi^*(-t)

$$

其中，$\psi^*(-t)$ 表示对 $\psi(t)$ 中的每个点取共轭复数。换句话说，复共轭小波基函数是通过将正向小波基函数 $\psi(t)$ 中的每个点取共轭而得到的。这意味着复共轭小波基函数在时间域上是对称的，其实部和虚部都是奇函数。

在小波变换的计算过程中，复共轭小波基函数与信号进行内积运算，用于计算小波变换的实部和虚部部分，从而获得更完整的频率信息。

复共轭小波基函数在小波变换的正交性和完备性等性质中起到重要作用，确保小波变换的精确性和有效性。同时，复共轭小波基函数也在许多小波变换的应用中发挥着重要的作用，如信号压缩、图像处理和特征提取等领域。

离散小波变换（Discrete Wavelet Transform，DWT）则是通过对信号进行多级的低通滤波和高通滤波得到的。对于长度为 $N$ 的离散信号 $x[n]$，DWT 的数学公式为：

$$

\begin{align*}

cA_{j}[k] &= \sum_{n=0}^{N-1} x[n] \cdot \frac{1}{\sqrt{2}} \cdot \psi \left( \frac{n-2k}{2^{j}} \right) \\

cD_{j}[k] &= \sum_{n=0}^{N-1} x[n] \cdot \frac{1}{\sqrt{2}} \cdot \phi \left( \frac{n-2k}{2^{j}} \right)

\end{align*}

$$

其中，$cA_{j}[k]$ 和 $cD_{j}[k]$ 分别是第 $j$ 级近似子带（Approximation Coefficients）和细节子带（Detail Coefficients），$k$ 表示子带的索引，$j$ 表示变换的级数。$\psi$ 和 $\phi$ 是小波基函数和尺度函数，$\cdot$ 表示卷积运算。

通过多级的低通和高通滤波，DWT 可以将信号分解为不同频率的子带，从而实现多尺度分析。同时，通过对子带系数进行阈值处理和逆变换，可以实现信号的压缩和去噪等应用。

综上所述，小波变换是一种灵活且强大的信号处理技术，可以用于时频分析、信号压缩、去噪和特征提取等多个领域。它的数学公式描述了小波基函数与信号之间的变换关系，通过这种变换可以获得信号的不同频率和时间尺度上的特征信息。

# 程序实现

实现二维离散小波变换（2D-DWT），我们可以使用Python的pywt库，它提供了对小波变换的支持。首先，确保已经安装了pywt库，可以使用以下命令安装：

```c
pip install PyWavelets
```


接下来，我们将给出一个示例代码，用于对一个二维图像进行小波变换。

```c
import pywt
import numpy as np
import matplotlib.pyplot as plt
# blur.png
image = cv2.imread('blur.png', 0)
# 进行二维离散小波变换（2D-DWT）
coeffs = pywt.dwt2(image, 'haar')
# 从结果中获取近似子带和细节子带
cA, (cH, cV, cD) = coeffs
# 打印结果
print("近似子带：")
print(cA)
print("\n水平细节子带：")
print(cH)
print("\n垂直细节子带：")
print(cV)
print("\n对角细节子带：")
print(cD)
# 可视化结果
plt.figure(figsize=(8, 8))
plt.subplot(2, 2, 1)
plt.imshow(cA, cmap='gray')
plt.title('Approximation')
plt.subplot(2, 2, 2)
plt.imshow(cH, cmap='gray')
plt.title('Horizontal Detail')
plt.subplot(2, 2, 3)
plt.imshow(cV, cmap='gray')
plt.title('Vertical Detail')
plt.subplot(2, 2, 4)
plt.imshow(cD, cmap='gray')
plt.title('Diagonal Detail')
plt.show()
```

![在这里插入图片描述](/static/img/images/04260.png)

# 子带


`pywt.dwt2`是`pywt`库中实现的二维离散小波变换（2D-DWT）函数。让我们通过数学公式来解释这个函数的作用：

给定一个2D数组`data`，我们可以将其表示为一个二维网格，其中每个元素表示一个像素的灰度值。2D-DWT的目标是将这个二维数组分解为多个子带，每个子带代表原始图像的不同部分或特征。具体地说，2D-DWT将输入的二维数组`data`分解为以下四个子带：

1. 近似子带（Approximation，记为`cA`）：表示原始图像的低频部分，包含图像中的整体结构和大致形状信息。

2. 水平细节子带（Horizontal Detail，记为`cH`）：表示图像中水平方向的高频信息，包括边缘和细节的水平变化。

3. 垂直细节子带（Vertical Detail，记为`cV`）：表示图像中垂直方向的高频信息，包括边缘和细节的垂直变化。

4. 对角细节子带（Diagonal Detail，记为`cD`）：表示图像中对角方向的高频信息，包括边缘和细节的对角变化。

这些子带可以用数学公式表示为：


```
cA：近似子带
cH：水平细节子带
cV：垂直细节子带
cD：对角细节子带
```


`pywt.dwt2`函数使用指定的小波（`wavelet`）对输入的图像`data`进行2D离散小波变换。其中，`mode`参数表示信号扩展模式，可以选择不同的模式进行计算。`axes`参数用于指定要进行变换的轴（即图像的维度）。函数返回近似子带`cA`和包含水平、垂直和对角细节子带的元组`cH, cV, cD`。

在示例中，我们使用Haar小波对其进行2D-DWT变换。

请注意，实际应用中，我们通常使用真实的图像数据，并根据不同的任务选择适合的小波函数。

# 数学公式





2D离散小波变换（2D-DWT）的数学公式可以用以下方式表示：

给定一个二维离散信号（图像） $f(x, y)$，其中 $(x, y)$ 表示图像的坐标，2D-DWT 将其分解为近似子带（Approximation, $cA$）和三个细节子带（Details）：水平细节子带（Horizontal Detail, $cH$）、垂直细节子带（Vertical Detail, $cV$）和对角细节子带（Diagonal Detail, $cD$）。

数学公式表示为：

$$

\begin{align*}

cA &= (cA_{LL}, cA_{LH}, cA_{HL}, cA_{HH}) \\

cH &= (cH_{LL}, cH_{LH}, cH_{HL}, cH_{HH}) \\

cV &= (cV_{LL}, cV_{LH}, cV_{HL}, cV_{HH}) \\

cD &= (cD_{LL}, cD_{LH}, cD_{HL}, cD_{HH})

\end{align*}

$$

其中，$cA_{LL}, cH_{LL}, cV_{LL}, cD_{LL}$ 是具有更低空间分辨率的近似子带和细节子带，分别代表水平、垂直和对角方向的低频信息。而 $cA_{LH}, cH_{LH}, cV_{LH}, cD_{LH}$ 分别代表水平、垂直和对角方向的高频信息。

在实际应用中，通常会对近似子带进行进一步的分解，即多级小波变换，以获得更多层次的细节信息。2D-DWT 是图像处理和压缩中常用的技术，它允许我们在不同频率和空间尺度上分析图像内容，并用于多种图像处理任务。

【Python】二维离散小波变换（2D-DWT）实现

p1点和p2点是一条线段的两端，沿着p1指向p2的方向，将线段长度延伸长度x，求延伸后的点ep。如下图：





![在这里插入图片描述](/static/img/images/04438.png)

Python代码：

```python
import math
def extend_line_segment(p1, p2, extension_length):
    """
    延伸线段长度并返回延伸后的点坐标
    参数:
    p1: list，起始点的坐标 [x1, y1]
    p2: list，结束点的坐标 [x2, y2]
    extension_length: float，延伸长度
    返回值:
    list，延伸后的点的坐标 [x, y]
    """
    # 计算p1指向p2的向量
    vector = [p2[0] - p1[0], p2[1] - p1[1]]
    # 计算向量的模
    magnitude = math.sqrt(vector[0] ** 2 + vector[1] ** 2)
    # 单位化向量
    unit_vector = [vector[0] / magnitude, vector[1] / magnitude]
    # 判断线段是否水平或垂直
    if p1[0] == p2[0]:  # 垂直线段
        extended_point = [p2[0], p2[1] + extension_length]
    elif p1[1] == p2[1]:  # 水平线段
        extended_point = [p2[0] + extension_length, p2[1]]
    else:  # 其他情况
        # 计算延伸后的点
        extended_point = [p2[0] + unit_vector[0] * extension_length, p2[1] + unit_vector[1] * extension_length]
    return extended_point
if __name__ == '__main__':
    p1 = [10, 200]
    p2 = [110, 100]
    extend_p = extend_line_segment(p1, p2, 300)
    print("延伸后的点坐标: ", extend_p)
    # 绘制图
    import matplotlib.pyplot as plt
    plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'r-')
    plt.plot([p2[0], extend_p[0]], [p2[1], extend_p[1]], 'b-')
    # 加注释
    plt.annotate('p1', xy=(p1[0], p1[1]), xytext=(p1[0], p1[1]))
    plt.annotate('p2', xy=(p2[0], p2[1]), xytext=(p2[0], p2[1]))
    plt.annotate('ep', xy=(extend_p[0], extend_p[1]), xytext=(extend_p[0] + 5, extend_p[1] + 5))
    # 等距
    plt.axis('equal')
    plt.show()
```

C++：

```cpp
cv::Point2f extend_line_segment(const cv::Point2f& p1, const cv::Point2f& p2, double extension_length) {
    // 计算p1指向p2的向量
    cv::Point2f vector = {p2.x - p1.x, p2.y - p1.y};
    // 计算向量的模
    double magnitude = sqrt(vector.x * vector.x + vector.y * vector.y);
    // 单位化向量
    cv::Point2f unit_vector = {static_cast<float>(vector.x / magnitude), static_cast<float>(vector.y / magnitude)};
    // 判断线段是否水平或垂直
    if (p1.x == p2.x) {  // 垂直线段
        cv::Point2f extended_point = {p2.x, static_cast<float>(p2.y + extension_length)};
        return extended_point;
    } else if (p1.y == p2.y) {  // 水平线段
        cv::Point2f extended_point = {static_cast<float>(p2.x + extension_length), p2.y};
        return extended_point;
    } else {  // 其他情况
        // 计算延伸后的点
        cv::Point2f extended_point = {static_cast<float>(p2.x + unit_vector.x * extension_length), static_cast<float>(p2.y + unit_vector.y * extension_length)};
        return extended_point;
    }
}
```

【几何数学】【Python】【C++】将线段沿着线段方向延长一定长度，求新的点

判断线段是否相交的办法（使用了向量叉积的方法）：





```c
首先，通过给定的线段端点坐标p1、p2、p3和p4构建了四个向量v1、v2、v3和v4：
v1表示从p1指向p2的向量，其分量为[p2[0] - p1[0], p2[1] - p1[1]]。
v2表示从p3指向p4的向量，其分量为[p4[0] - p3[0], p4[1] - p3[1]]。
v3表示从p1指向p3的向量，其分量为[p3[0] - p1[0], p3[1] - p1[1]]。
v4表示从p1指向p4的向量，其分量为[p4[0] - p1[0], p4[1] - p1[1]]。
接下来，计算了两个叉积cross1和cross2：
cross1表示v1和v3的叉积，计算公式为v1[0] * v3[1] - v1[1] * v3[0]。
cross2表示v1和v4的叉积，计算公式为v1[0] * v4[1] - v1[1] * v4[0]。
最后，根据两个叉积的乘积进行判断：
如果cross1和cross2的乘积小于等于0，意味着v1和v3位于不同的半平面或者其中一个线段的某个端点在另一个线段上，这时可以判断两条线段相交。
如果cross1和cross2的乘积大于0，意味着v1和v3位于同一侧或者两个线段没有交点，这时可以判断两条线段不相交。
根据以上逻辑，如果条件满足，则返回True表示线段相交，否则返回False表示线段不相交。
```


Python代码实现：

```python
class point():  # 定义类
    def __init__(self, x, y):
        self.x = x
        self.y = y
def cross(p1, p2, p3):  # 跨立实验
    x1 = p2.x - p1.x
    y1 = p2.y - p1.y
    x2 = p3.x - p1.x
    y2 = p3.y - p1.y
    return x1 * y2 - x2 * y1
def has_intersect(p1, p2, p3, p4):  # 判断两线段是否相交
    # 快速排斥，以l1、l2为对角线的矩形必相交，否则两线段不相交
    if (max(p1.x, p2.x) >= min(p3.x, p4.x)  # 矩形1最右端大于矩形2最左端
            and max(p3.x, p4.x) >= min(p1.x, p2.x)  # 矩形2最右端大于矩形最左端
            and max(p1.y, p2.y) >= min(p3.y, p4.y)  # 矩形1最高端大于矩形最低端
            and max(p3.y, p4.y) >= min(p1.y, p2.y)):  # 矩形2最高端大于矩形最低端
        # 若通过快速排斥则进行跨立实验
        if (cross(p1, p2, p3) * cross(p1, p2, p4) <= 0
                and cross(p3, p4, p1) * cross(p3, p4, p2) <= 0):
            D = True
        else:
            D = False
    else:
        D = False
    return D
def are_lines_intersect(p1, p2, p3, p4):
    p1 = point(p1[0], p1[1])
    p2 = point(p2[0], p2[1])
    p3 = point(p3[0], p3[1])
    p4 = point(p4[0], p4[1])
    if has_intersect(p1, p2, p3, p4):
        return True
    else:
        return False
def compute_intersection(p1, p2, p3, p4):
    if p1[0] == p2[0]:  # p1p2为垂直线
        x = p1[0]
        slope2 = (p4[1] - p3[1]) / (p4[0] - p3[0])
        intercept2 = p3[1] - slope2 * p3[0]
        y = slope2 * x + intercept2
    elif p3[0] == p4[0]:  # p3p4为垂直线
        x = p3[0]
        slope1 = (p2[1] - p1[1]) / (p2[0] - p1[0])
        intercept1 = p1[1] - slope1 * p1[0]
        y = slope1 * x + intercept1
    else:
        slope1 = (p2[1] - p1[1]) / (p2[0] - p1[0])
        slope2 = (p4[1] - p3[1]) / (p4[0] - p3[0])
        intercept1 = p1[1] - slope1 * p1[0]
        intercept2 = p3[1] - slope2 * p3[0]
        if slope1 == slope2:
            # 斜率相等的时候，如果线段重合，交点是无数个，这里返回断点表达此意
            if p1[0] <= p3[0] <= p2[0]:
                return p3
            if p1[0] <= p4[0] <= p2[0]:
                return p4
        else:
            x = (intercept2 - intercept1) / (slope1 - slope2)
            y = slope1 * x + intercept1
    return [x, y]
if __name__ == '__main__':
    p1 = [0, 5]
    p2 = [10, 5]
    p3 = [0, 10]
    p4 = [10, 10]
    # p1 p2 是一条线段
    # p3 p4 是另一条线段
    # 判断是否相交，相交且求交点
    if are_lines_intersect(p1, p2, p3, p4):
        print(compute_intersection(p1, p2, p3, p4))
        x, y = compute_intersection(p1, p2, p3, p4)
    else:
        print("No intersection")
    # 画图
    import matplotlib.pyplot as plt
    # 样式是波浪线
    plt.plot([p1[0], p2[0]], [p1[1], p2[1]], color='r')
    # 样式是点线
    plt.plot([p3[0], p4[0]], [p3[1], p4[1]], color='b', linewidth=4, linestyle=':')
    # 加注释
    plt.annotate('p1', xy=(p1[0], p1[1]), xytext=(p1[0] + 10, p1[1] + 10))
    plt.annotate('p2', xy=(p2[0], p2[1]), xytext=(p2[0] + 10, p2[1] + 10))
    plt.annotate('p3', xy=(p3[0], p3[1]), xytext=(p3[0] + 10, p3[1] + 10))
    plt.annotate('p4', xy=(p4[0], p4[1]), xytext=(p4[0] + 10, p4[1] + 10))
    try:
        # 绘制交点
        plt.scatter(x, y, color='g', marker='o')
        # 给交点加注释
        plt.annotate('intersection', xy=(x, y), xytext=(x, y),
                     arrowprops=dict(facecolor='g', shrink=0.05))
    except:
        pass
    # 坐标轴等距
    plt.axis('equal')
    plt.show()
```

绘制的图：

![在这里插入图片描述](/static/img/images/04451.png)

![在这里插入图片描述](/static/img/images/04452.png)

不相交：

![在这里插入图片描述](/static/img/images/04453.png)


# C++ 借助opencv的数据结构

```cpp
float cross(const cv::Point2f& p1, const cv::Point2f& p2, const cv::Point2f& p3) {
    float x1 = p2.x - p1.x;
    float y1 = p2.y - p1.y;
    float x2 = p3.x - p1.x;
    float y2 = p3.y - p1.y;
    return x1 * y2 - x2 * y1;
}
bool has_intersect(const cv::Point2f& p1, const cv::Point2f& p2, const cv::Point2f& p3, const cv::Point2f& p4) {
    // 快速排斥，以l1、l2为对角线的矩形必相交，否则两线段不相交
    if (std::max(p1.x, p2.x) >= std::min(p3.x, p4.x)  // 矩形1最右端大于矩形2最左端
        && std::max(p3.x, p4.x) >= std::min(p1.x, p2.x)  // 矩形2最右端大于矩形最左端
        && std::max(p1.y, p2.y) >= std::min(p3.y, p4.y)  // 矩形1最高端大于矩形最低端
        && std::max(p3.y, p4.y) >= std::min(p1.y, p2.y)) {  // 矩形2最高端大于矩形最低端
        // 若通过快速排斥则进行跨立实验
        if (cross(p1, p2, p3) * cross(p1, p2, p4) <= 0
            && cross(p3, p4, p1) * cross(p3, p4, p2) <= 0) {
            return true;
        }
        else {
            return false;
        }
    }
    else {
        return false;
    }
}
bool has_intersect(const std::vector<cv::Point2f>& seg1, const std::vector<cv::Point2f>& seg2) {
    return has_intersect(seg1[0], seg1[1], seg2[0], seg2[1]);
}
// compute_intersection: 计算两条线段的交点
// 原理：计算两条线段的斜率和截距，然后解方程
cv::Point2f compute_intersection(cv::Point2f p1, cv::Point2f p2, cv::Point2f p3, cv::Point2f p4) {
    cv::Point2f intersection;
    if (p1.x == p2.x) {
        intersection.x = p1.x;
        float slope2 = (p4.y - p3.y) / (p4.x - p3.x);
        float intercept2 = p3.y - slope2 * p3.x;
        intersection.y = slope2 * intersection.x + intercept2;
    } else if (p3.x == p4.x) {
        intersection.x = p3.x;
        float slope1 = (p2.y - p1.y) / (p2.x - p1.x);
        float intercept1 = p1.y - slope1 * p1.x;
        intersection.y = slope1 * intersection.x + intercept1;
    } else {
        float slope1 = (p2.y - p1.y) / (p2.x - p1.x);
        float slope2 = (p4.y - p3.y) / (p4.x - p3.x);
        float intercept1 = p1.y - slope1 * p1.x;
        float intercept2 = p3.y - slope2 * p3.x;
        if (slope1 == slope2) {
            // if p1[0] <= p3[0] <= p2[0]:
            if (p1.x <= p3.x && p3.x <= p2.x) {
                return p3;
            } else if (p1.x <= p4.x && p4.x <= p2.x) {
                return p4;
            } else {
                return {-1, -1};
            }
        }
    }
    return intersection;
}
```

【几何数学】【Python】【C++】判断两条线段是否相交，若相交则求出交点坐标

一个高中数学题目，已经点P1和点P2构成直线，经过P3点做已知直线的垂线，求垂线与已知直线的交点坐标。





![在这里插入图片描述](/static/img/images/04471.png)


```python
p1 = [100, 15]
p2 = [16, 85]
p3 = [-50, 100]
if p2[0] - p1[0] == 0:
    # p1 p2 构成垂线，那么垂直线就是一条水平线
    x = p1[0]
    y = p3[1]
elif p2[1] - p1[1] == 0:
    # p1 p2 构成水平线，那么垂直线就是一条垂直线
    x = p3[0]
    y = p1[1]
else:
    slope = (p2[1] - p1[1]) / (p2[0] - p1[0])
    perpendicularSlope = -1 / slope
    intercept = p3[1] - perpendicularSlope * p3[0]
    x = (p3[1] - p1[1] + slope * p1[0] - perpendicularSlope * p3[0]) / (slope - perpendicularSlope)
    y = slope * (x - p1[0]) + p1[1]
# 绘制两条直线和交点
import matplotlib.pyplot as plt
plt.figure(figsize=(10, 10))
plt.plot([p1[0], p2[0]], [p1[1], p2[1]], color='b')
plt.plot([p3[0], x], [p3[1], y], color='r')
plt.scatter([x], [y], color='r')
# 给P1,P2,P3点加注释
plt.annotate('P1', xy=(p1[0], p1[1]), xytext=(p1[0] + 0.5, p1[1] + 0.5))
plt.annotate('P2', xy=(p2[0], p2[1]), xytext=(p2[0] + 0.5, p2[1] + 0.5))
plt.annotate('P3', xy=(p3[0], p3[1]), xytext=(p3[0] + 0.5, p3[1] + 0.5))
# 给交点加注释
plt.annotate('JD', xy=(x, y), xytext=(x + 0.5, y + 0.5))
# 让坐标轴比例一样
plt.axis('equal')
plt.show()
```

【Python】经过一个点P3的一条直线垂直于已知直线，求交点坐标

我是一个专注技术分享的博主,尤其爱搞深度学习和单片机电子。你现在看到的是我的个人博客网站。