• sfm算法之三角化(三角测量)

            sfm算法流程一般是特征点提取、特征点匹配、计算本质矩阵/基础矩阵，最后三角化。但是利用机械臂去观察周围，前后帧姿态变化参数是具有的，所以不需要通过基础矩阵获取。

            即利用机械臂的信息直接进行深度估计。已知：手眼标定、相机外参（利用机械臂可获取）、两次拍摄图片目标的像素位置。求目标深度。目前从几次测试来看，效果还是可信的，具体误差没有评估。

            利用的公式如下：

     

    1. #!/usr/bin/env python
    2. # -*- coding: utf-8 -*-
    3. # @Time        :2022/7/27 14:59
    4. # @Author      :weiz
    5. # @ProjectName :robotVision3
    6. # @File        :triangulation.py
    7. # @Description :
    8. import cv2
    9.  
    10. from triangulation_data import *
    11. import math
    12.  
    13.  
    14. def get_M(R, t):
    15.     """
    16.     获取旋转平移的矩阵
    17.     :param R:
    18.     :param t:
    19.     :return:
    20.     """
    21.     M = [[R[0][0], R[0][1], R[0][2], t[0]],
    22.          [R[1][0], R[1][1], R[1][2], t[1]],
    23.          [R[2][0], R[2][1], R[2][2], t[2]]]
    24.     return np.array(M)
    25.  
    26.  
    27.  
    28. def get_M_homo(R, t):
    29.     """
    30.     获取旋转平移的齐次矩阵
    31.     :param R:
    32.     :param t:
    33.     :return:
    34.     """
    35.     M = [[R[0][0], R[0][1], R[0][2], t[0]],
    36.          [R[1][0], R[1][1], R[1][2], t[1]],
    37.          [R[2][0], R[2][1], R[2][2], t[2]],
    38.          [0,       0,       0,       1]]
    39.     return np.array(M)
    40.  
    41.  
    42. def get_world2point2_R_t(R_camera2gripper, t_camera2gripper, point1_gripper, point2_gripper):
    43.     """
    44.     获取关键点1坐标系到关键点2坐标系的 R,t
    45.     :param R_camera2gripper:手眼标定的R
    46.     :param t_camera2gripper:手眼标定的t
    47.     :param point1_gripper:视点1时刻机械臂末端的位姿[x,y,z,rx,ry,rz]
    48.     :param point2_gripper:视点2时刻机械臂末端的位姿[x,y,z,rx,ry,rz]
    49.     :return:
    50.     """
    51.     R_gripper2base_point1 = np.zeros((3, 3))
    52.     t_gripper2base_point1 = np.array([point1_gripper[0], point1_gripper[1], point1_gripper[2]])
    53.     cv2.Rodrigues(np.array([point1_gripper[3], point1_gripper[4], point1_gripper[5]]), R_gripper2base_point1)
    54.  
    55.     R_gripper2base_point2 = np.zeros((3, 3))
    56.     t_gripper2base_point2 = np.array([point2_gripper[0], point2_gripper[1], point2_gripper[2]])
    57.     cv2.Rodrigues(np.array([point2_gripper[3], point2_gripper[4], point2_gripper[5]]), R_gripper2base_point2)
    58.  
    59.     R_gripper2camera = np.linalg.inv(np.array(R_camera2gripper))
    60.     t_gripper2camera = -np.dot(R_gripper2camera, t_camera2gripper)
    61.  
    62.     R_base2gripper_point2 = np.linalg.inv(np.array(R_gripper2base_point2))
    63.     t_base2gripper_point2 = -np.dot(R_base2gripper_point2, t_gripper2base_point2)
    64.  
    65.     M = get_M_homo(R_camera2gripper, t_camera2gripper)
    66.     M = np.matmul(get_M_homo(R_gripper2base_point1, t_gripper2base_point1), M)
    67.     M = np.matmul(get_M_homo(R_base2gripper_point2, t_base2gripper_point2), M)
    68.     M = np.matmul(get_M_homo(R_gripper2camera, t_gripper2camera), M)
    69.  
    70.     R_world2point2 = M[0:3, 0:3]
    71.     t_world2point2 = np.array([M[0][3], M[1][3], M[2][3]])
    72.     # print(M)
    73.     # print(R_world2point2)
    74.     # print(t_world2point2)
    75.  
    76.     return R_world2point2, t_world2point2, M
    77.  
    78.  
    79. def triangulation(point1, point2, ipm, R_world2point2, t_world2point2):
    80.     """
    81.     三角测量：利用机械臂位姿信息直接进行三维坐标估计
    82.     :param point1:该坐标系为世界坐标系
    83.     :param point2:
    84.     :param ipm:相机内参
    85.     :param R_world2point2:
    86.     :param t_world2point2:
    87.     :return:
    88.     """
    89.     ipm = np.array(ipm)
    90.     M1 = np.matmul(ipm, get_M(np.eye(3, 3), [0, 0, 0]))
    91.     M2 = np.matmul(ipm, get_M(R_world2point2, t_world2point2))
    92.     A = []
    93.     A.append(point1[0] * M1[2] - M1[0])
    94.     A.append(point1[1] * M1[2] - M1[1])
    95.     A.append(point2[0] * M2[2] - M2[0])
    96.     A.append(point2[1] * M2[2] - M2[1])
    97.     # print(np.array(A))
    98.     U, sigma, V = np.linalg.svd(A)
    99.     p_1 = [V[3][0], V[3][1], V[3][2], V[3][3]]
    100.     p_2 = [V[3][0] / V[3][3], V[3][1] / V[3][3], V[3][2] / V[3][3]]
    101.     # print(p_1)
    102.     # print(p_2)
    103.  
    104.     total_error = 0
    105.     point1_pro, _ = cv2.projectPoints(np.array([p_2]), np.eye(3), np.array([[0.], [0.], [0.]]), ipm, None)
    106.     point2_pro, _ = cv2.projectPoints(np.array([p_2]), R_world2point2, t_world2point2, ipm, None)
    107.     print(point2_pro)
    108.     total_error = total_error + cv2.norm(np.array([[point1]], dtype=np.double), point1_pro, cv2.NORM_L2) / 1.
    109.     total_error = total_error + cv2.norm(np.array([[point1]], dtype=np.float64), point2_pro, cv2.NORM_L2) / 1.
    110.     print(total_error)
    111.  
    112.     return p_2
    113.  
    114.  
    115. def triangulation_opencv(point1, point2, ipm, R_world2point2, t_world2point2):
    116.     point1 = np.array([point1], dtype=np.float32)
    117.     point2 = np.array([point2], dtype=np.float32)
    118.  
    119.     M1 = np.zeros((3, 4))
    120.     M2 = np.zeros((3, 4))
    121.     M1[0:3, 0:3] = np.eye(3)
    122.     M2[0:3, 0:3] = np.float32(R_world2point2)
    123.     M2[:, 3] = np.float32(t_world2point2)
    124.     M1 = np.matmul(ipm, M1)
    125.     M2 = np.matmul(ipm, M2)
    126.     p_homo = cv2.triangulatePoints(M1, M2, point1.T, point2.T)
    127.     p = []
    128.     # 齐次坐标转三维坐标：前三个维度除以第四个维度
    129.     for i in range(len(p_homo[0])):
    130.         col = p_homo[:, i]
    131.         col /= col[3]
    132.         p.append([col[0], col[1], col[2]])
    133.     return p[0]
    134.  
    135.  
    136.  
    137.  
    138.  
    139. R_camera2gripper =[[-0.999266726198567, -0.016016251269208765, -0.0347777171142492],
    140.                    [-0.03487664683144269, 0.005939423009193905, 0.99937397542667],
    141.                    [-0.015799665129108013, 0.9998540908300567, -0.00649366092498505]]
    142. t_camera2gripper = [0.057164748537088694, 0.10581519613132581, 0.1255394553568957]
    143. ipm = np.array([[535.05, 0, 653.005], [0, 535.01, 386.191], [0, 0, 1]])
    144. if __name__ == "__main__":
    145.     R_world2point2, t_world2point2, M = get_world2point2_R_t(R_camera2gripper, t_camera2gripper, point1_gripper, point2_gripper)
    146.     p = triangulation(point1, point2, ipm, R_world2point2, t_world2point2)
    147.     print(p)
    148.  
    149.     p = triangulation_opencv(point1, point2, ipm, R_world2point2, t_world2point2)
    150.     print(p)

    数据：

    1. #!/usr/bin/env python
    2. # -*- coding: utf-8 -*-
    3. # @Time        :2022/7/29 9:28
    4. # @Author      :weiz
    5. # @ProjectName :Sfm-python-master
    6. # @File        :triangulation_data.py
    7. # @Description :
    8. # Copyright (C) 2021-2025 Jiangxi Institute Of Intelligent Industry Technology Innovation
    9. import numpy as np
    10.  
    11.  
    12. point1 = [738, 389]  # 0.5410932
    13. point1_gripper = [0.5122332451863898, 0.5128182769039566, 0.9630757531852082, -0.7115522167741687, -1.7916510038378088, 1.7013068394360287]
    14. point2 = [797, 374]
    15. point2_gripper = [0.3820852469525631, 0.5235437570806256, 0.9711089613285708, -0.7071726699555351, -1.9459688091738208, 1.7263544531858268]
    16.  
    17. # point1 = [797, 374]  # 0.54940385
    18. # point1_gripper = [0.3820852469525631, 0.5235437570806256, 0.9711089613285708, -0.7071726699555351, -1.9459688091738208, 1.7263544531858268]
    19. # point2 = [715, 343]
    20. # point2_gripper = [0.5056936674804589, 0.4650883838281726, 1.0634729391460807, -0.6999271853483486, -1.749012465435611, 1.6882826312357782]
    21.  
    22. # point1 = [715, 343]  # 0.64878213
    23. # point1_gripper = [0.5056936674804589, 0.4650883838281726, 1.0634729391460807, -0.6999271853483486, -1.749012465435611, 1.6882826312357782]
    24. # point2 = [733, 368]
    25. # point2_gripper = [0.5485518509025477, 0.5157901201998042, 0.9837568437877331, -0.6609672121210775, -1.7084733902118903, 1.7261963960916609]
    26.  
    27. # point1 = [733, 368]  # 0.5727386
    28. # point1_gripper = [0.5485518509025477, 0.5157901201998042, 0.9837568437877331, -0.6609672121210775, -1.7084733902118903, 1.7261963960916609]
    29. # point2 = [738, 389]
    30. # point2_gripper = [0.5122332451863898, 0.5128182769039566, 0.9630757531852082, -0.7115522167741687, -1.7916510038378088, 1.7013068394360287]
    31.  
    32.  
    33. def pixel_normalization(point1, point2):
    34.     points_norm = []
    35.     points_norm.append(point1)
    36.     points_norm.append(point2)
    37.     points_norm = np.array(points_norm)
    38.     points_norm = (points_norm - np.mean(points_norm)) / np.std(points_norm)
    39.     return points_norm[0], points_norm[1]
    40.  
    41.  
    42. if __name__ == "__main__":
    43.     p1, p2 = pixel_normalization(point1, point2)
    44.     print(p1)
    45.     print(p2)

  • 相关阅读:
    mac安装python 和 pip
    Windows上安装jdk17后没有jre的解决方法
    车载T-BOX
    论坛介绍｜COSCon'23开源商业（V）
    Python进阶教程：pandas数据分析实践示例总结
    Hbuilder打包成APP流程,以及遇到的坑
    多线程详细介绍
    Linux多线程服务端编程：使用muduo C++网络库 学习笔记 第四章 C++多线程系统编程精要
    Web渗透_手动漏洞挖掘
    13.linux进程基础
  • 原文地址：https://blog.csdn.net/qq_31112205/article/details/126466206