根据三点坐标计算角度(python,三维坐标,向量法)

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@Time     : 2021/8/10 9:16
@Author   : Allen Pen(Haijun Peng)
@E-mail   : shengyutou@outlook.com
NOTE:
"""
import math


def cal_angle(point_a, point_b, point_c):
    """
    根据三点坐标计算夹角

                  点a
           点b ∠
                   点c

    :param point_a、point_b、point_c: 数据类型为list,二维坐标形式[x、y]或三维坐标形式[x、y、z]
    :return: 返回角点b的夹角值


    数学原理:
    设m,n是两个不为0的向量,它们的夹角为<m,n> (或用α ,β, θ ,..,字母表示)

    1、由向量公式:cos<m,n>=m.n/|m||n|

    2、若向量用坐标表示,m=(x1,y1,z1), n=(x2,y2,z2),

    则,m.n=(x1x2+y1y2+z1z2).

    |m|=√(x1^2+y1^2+z1^2), |n|=√(x2^2+y2^2+z2^2).

    将这些代入②得到:

    cos<m,n>=(x1x2+y1y2+z1z2)/[√(x1^2+y1^2+z1^2)*√(x2^2+y2^2+z2^2)]

    上述公式是以空间三维坐标给出的,令坐标中的z=0,则得平面向量的计算公式。

    两个向量夹角的取值范围是:[0,π].

    夹角为锐角时,cosθ>0;夹角为钝角时,cosθ<0.

    """
    a_x, b_x, c_x = point_a[0], point_b[0], point_c[0]  # 点a、b、c的x坐标
    a_y, b_y, c_y = point_a[1], point_b[1], point_c[1]  # 点a、b、c的y坐标

    if len(point_a) == len(point_b) == len(point_c) == 3:
        # print("坐标点为3维坐标形式")
        a_z, b_z, c_z = point_a[2], point_b[2], point_c[2]  # 点a、b、c的z坐标
    else:
        a_z, b_z, c_z = 0,0,0  # 坐标点为2维坐标形式,z 坐标默认值设为0
        # print("坐标点为2维坐标形式,z 坐标默认值设为0")

    # 向量 m=(x1,y1,z1), n=(x2,y2,z2)
    x1,y1,z1 = (a_x-b_x),(a_y-b_y),(a_z-b_z)
    x2,y2,z2 = (c_x-b_x),(c_y-b_y),(c_z-b_z)

    # 两个向量的夹角,即角点b的夹角余弦值
    cos_b = (x1*x2 + y1*y2 + z1*z2) / (math.sqrt(x1**2 + y1**2 + z1**2) *(math.sqrt(x2**2 + y2**2 + z2**2))) # 角点b的夹角余弦值
    B = math.degrees(math.acos(cos_b)) # 角点b的夹角值
    return B

cal_angle((3**0.5,1), (0,0), (3**0.5,0))  # 结果为 30°
cal_angle((1,1), (0,0), (1,0))  # 结果为 45°
cal_angle((-1,1), (0,0), (1,0)) # 结果为 135°

  • 15
    点赞
  • 67
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
EPnP(Efficient Perspective-n-Point)是一种用于计算相机的姿态和三维世界坐标之间关系的求解方。在EPnP中,根据相机的内参矩阵和2D像素坐标,可以计算出相机的旋转矩阵和平移向量,进而推导出真实世界中的三维物体坐标。 在Python中,可以使用OpenCV库来实现EPnP的代码。下面是一个简单的示例代码: ```python import cv2 import numpy as np # 2D像素坐标 image_points = np.array([[100, 200], [300, 400], [500, 600]], dtype=np.float32) # 对应的真实三维坐标 world_points = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype=np.float32) # 相机内参矩阵 camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float32) # 失真系数 dist_coeffs = np.zeros((4, 1)) # 使用EPnP求解姿态矩阵和三维坐标 _, rvec, tvec, _ = cv2.solvePnPRansac(world_points, image_points, camera_matrix, dist_coeffs) # 旋转向量转换为旋转矩阵 rot_matrix, _ = cv2.Rodrigues(rvec) # 输出结果 print("旋转矩阵:") print(rot_matrix) print("平移向量:") print(tvec) ``` 在这个示例代码中,我们首先定义了2D像素坐标和对应的真实三维坐标,然后定义了相机的内参矩阵和失真系数。接下来,使用`cv2.solvePnPRansac`函数来求解姿态矩阵和三维坐标。最后,将旋转向量转换为旋转矩阵,并输出结果。 当然,实际应用时,我们还需要进行一些图像预处理、特征提取和匹配等步骤,来得到准确的2D像素坐标。这里只是简单展示了EPnP的实现过程,具体的应用场景还需要根据实际情况进行调整和优化。
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值