平面拟合之三个三维坐标确定平面

1.简介

  在slam任务中，需要求解物体的姿态信息，这就需要求解平面的法向量，这个专栏，会持续更新物体姿态求解的文章，欢迎大家点赞收藏哈!

2.定义

  三个不共线的点确定一个平面，平面方程：Ax+By+Cz+D=0

3.代码

  已知三个不共线点，求解平面方程和平面法向量，代码如下：

import numpy as np

def points_to_plane(point1, point2, point3):
    """
    根据三个空间点求平面方程

    Args:
        point1 (list or tuple): 第一个空间点的坐标 [x1, y1, z1]
        point2 (list or tuple): 第二个空间点的坐标 [x2, y2, z2]
        point3 (list or tuple): 第三个空间点的坐标 [x3, y3, z3]

    Returns:
        tuple: 平面方程的系数 (A, B, C, D),表示 Ax + By + Cz + D = 0
    """
    # 将点坐标转换为 NumPy 数组
    p1 = np.array(point1)
    p2 = np.array(point2)
    p3 = np.array(point3)

    # 计算两个向量
    v1 = p2 - p1
    v2 = p3 - p1

    # 计算法向量
    normal = np.cross(v1, v2)
    A, B, C = normal

    # 计算 D
    D = -np.dot(normal, p1)

    return A, B, C, D

def plane_normal(A, B, C):
    """
    根据平面方程的系数求平面的单位法向量

    Args:
        A (float): 平面方程中的系数 A
        B (float): 平面方程中的系数 B
        C (float): 平面方程中的系数 C

    Returns:
        numpy.ndarray: 平面的单位法向量
    """
    # 创建法向量
    normal = np.array([A, B, C])

    # 归一化法向量
    unit_normal = normal / np.linalg.norm(normal)

    return unit_normal

# 示例用法
if __name__ == "__main__":
    # 输入三个空间点的坐标
    point1 = [1.28, 2.42, 3.17]
    point2 = [4.15, 5.23, 6.48]
    point3 = [7.23, 8.10, 9.26]

    # 计算平面方程
    A, B, C, D = points_to_plane(point1, point2, point3)

    # 输出平面方程
    print("平面方程: {}x + {}y + {}z + {} = 0".format(A, B, C, D))

    # 计算平面的单位法向量
    unit_normal = plane_normal(A, B, C)

    # 输出平面的单位法向量
    print("平面的单位法向量:", unit_normal)

4.运行结果

  运行结果如下：