The SFM algorithm process generally includes feature point extraction, feature point matching, calculation of essential matrix/basic matrix, and finally triangulation. However, when using a robotic arm to observe the surroundings, the posture change parameters of the front and rear frames are available, so there is no need to obtain them through the basic matrix.
That is, the information of the robotic arm is used to directly estimate the depth. Known: hand-eye calibration, camera external parameters (obtainable using a robotic arm), and pixel positions of the target in the two captured pictures. Find the target depth. At present, judging from several tests, the effect is still credible, and the specific error has not been evaluated.
The formula used is as follows:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time :2022/7/27 14:59
# @Author :weiz
# @ProjectName :robotVision3
# @File :triangulation.py
# @Description :
import cv2
from triangulation_data import *
import math
def get_M(R, t):
"""
获取旋转平移的矩阵
:param R:
:param t:
:return:
"""
M = [[R[0][0], R[0][1], R[0][2], t[0]],
[R[1][0], R[1][1], R[1][2], t[1]],
[R[2][0], R[2][1], R[2][2], t[2]]]
return np.array(M)
def get_M_homo(R, t):
"""
获取旋转平移的齐次矩阵
:param R:
:param t:
:return:
"""
M = [[R[0][0], R[0][1], R[0][2], t[0]],
[R[1][0], R[1][1], R[1][2], t[1]],
[R[2][0], R[2][1], R[2][2], t[2]],
[0, 0, 0, 1]]
return np.array(M)
def get_world2point2_R_t(R_camera2gripper, t_camera2gripper, point1_gripper, point2_gripper):
"""
获取关键点1坐标系到关键点2坐标系的 R,t
:param R_camera2gripper:手眼标定的R
:param t_camera2gripper:手眼标定的t
:param point1_gripper:视点1时刻机械臂末端的位姿[x,y,z,rx,ry,rz]
:param point2_gripper:视点2时刻机械臂末端的位姿[x,y,z,rx,ry,rz]
:return:
"""
R_gripper2base_point1 = np.zeros((3, 3))
t_gripper2base_point1 = np.array([point1_gripper[0], point1_gripper[1], point1_gripper[2]])
cv2.Rodrigues(np.array([point1_gripper[3], point1_gripper[4], point1_gripper[5]]), R_gripper2base_point1)
R_gripper2base_point2 = np.zeros((3, 3))
t_gripper2base_point2 = np.array([point2_gripper[0], point2_gripper[1], point2_gripper[2]])
cv2.Rodrigues(np.array([point2_gripper[3], point2_gripper[4], point2_gripper[5]]), R_gripper2base_point2)
R_gripper2camera = np.linalg.inv(np.array(R_camera2gripper))
t_gripper2camera = -np.dot(R_gripper2camera, t_camera2gripper)
R_base2gripper_point2 = np.linalg.inv(np.array(R_gripper2base_point2))
t_base2gripper_point2 = -np.dot(R_base2gripper_point2, t_gripper2base_point2)
M = get_M_homo(R_camera2gripper, t_camera2gripper)
M = np.matmul(get_M_homo(R_gripper2base_point1, t_gripper2base_point1), M)
M = np.matmul(get_M_homo(R_base2gripper_point2, t_base2gripper_point2), M)
M = np.matmul(get_M_homo(R_gripper2camera, t_gripper2camera), M)
R_world2point2 = M[0:3, 0:3]
t_world2point2 = np.array([M[0][3], M[1][3], M[2][3]])
# print(M)
# print(R_world2point2)
# print(t_world2point2)
return R_world2point2, t_world2point2, M
def triangulation(point1, point2, ipm, R_world2point2, t_world2point2):
"""
三角测量：利用机械臂位姿信息直接进行三维坐标估计
:param point1:该坐标系为世界坐标系
:param point2:
:param ipm:相机内参
:param R_world2point2:
:param t_world2point2:
:return:
"""
ipm = np.array(ipm)
M1 = np.matmul(ipm, get_M(np.eye(3, 3), [0, 0, 0]))
M2 = np.matmul(ipm, get_M(R_world2point2, t_world2point2))
A = []
A.append(point1[0] * M1[2] - M1[0])
A.append(point1[1] * M1[2] - M1[1])
A.append(point2[0] * M2[2] - M2[0])
A.append(point2[1] * M2[2] - M2[1])
# print(np.array(A))
U, sigma, V = np.linalg.svd(A)
p_1 = [V[3][0], V[3][1], V[3][2], V[3][3]]
p_2 = [V[3][0] / V[3][3], V[3][1] / V[3][3], V[3][2] / V[3][3]]
# print(p_1)
# print(p_2)
total_error = 0
point1_pro, _ = cv2.projectPoints(np.array([p_2]), np.eye(3), np.array([[0.], [0.], [0.]]), ipm, None)
point2_pro, _ = cv2.projectPoints(np.array([p_2]), R_world2point2, t_world2point2, ipm, None)
print(point2_pro)
total_error = total_error + cv2.norm(np.array([[point1]], dtype=np.double), point1_pro, cv2.NORM_L2) / 1.
total_error = total_error + cv2.norm(np.array([[point1]], dtype=np.float64), point2_pro, cv2.NORM_L2) / 1.
print(total_error)
return p_2
def triangulation_opencv(point1, point2, ipm, R_world2point2, t_world2point2):
point1 = np.array([point1], dtype=np.float32)
point2 = np.array([point2], dtype=np.float32)
M1 = np.zeros((3, 4))
M2 = np.zeros((3, 4))
M1[0:3, 0:3] = np.eye(3)
M2[0:3, 0:3] = np.float32(R_world2point2)
M2[:, 3] = np.float32(t_world2point2)
M1 = np.matmul(ipm, M1)
M2 = np.matmul(ipm, M2)
p_homo = cv2.triangulatePoints(M1, M2, point1.T, point2.T)
p = []
# 齐次坐标转三维坐标：前三个维度除以第四个维度
for i in range(len(p_homo[0])):
col = p_homo[:, i]
col /= col[3]
p.append([col[0], col[1], col[2]])
return p[0]
R_camera2gripper =[[-0.999266726198567, -0.016016251269208765, -0.0347777171142492],
[-0.03487664683144269, 0.005939423009193905, 0.99937397542667],
[-0.015799665129108013, 0.9998540908300567, -0.00649366092498505]]
t_camera2gripper = [0.057164748537088694, 0.10581519613132581, 0.1255394553568957]
ipm = np.array([[535.05, 0, 653.005], [0, 535.01, 386.191], [0, 0, 1]])
if __name__ == "__main__":
R_world2point2, t_world2point2, M = get_world2point2_R_t(R_camera2gripper, t_camera2gripper, point1_gripper, point2_gripper)
p = triangulation(point1, point2, ipm, R_world2point2, t_world2point2)
print(p)
p = triangulation_opencv(point1, point2, ipm, R_world2point2, t_world2point2)
print(p)data:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time :2022/7/29 9:28
# @Author :weiz
# @ProjectName :Sfm-python-master
# @File :triangulation_data.py
# @Description :
# Copyright (C) 2021-2025 Jiangxi Institute Of Intelligent Industry Technology Innovation
import numpy as np
point1 = [738, 389] # 0.5410932
point1_gripper = [0.5122332451863898, 0.5128182769039566, 0.9630757531852082, -0.7115522167741687, -1.7916510038378088, 1.7013068394360287]
point2 = [797, 374]
point2_gripper = [0.3820852469525631, 0.5235437570806256, 0.9711089613285708, -0.7071726699555351, -1.9459688091738208, 1.7263544531858268]
# point1 = [797, 374] # 0.54940385
# point1_gripper = [0.3820852469525631, 0.5235437570806256, 0.9711089613285708, -0.7071726699555351, -1.9459688091738208, 1.7263544531858268]
# point2 = [715, 343]
# point2_gripper = [0.5056936674804589, 0.4650883838281726, 1.0634729391460807, -0.6999271853483486, -1.749012465435611, 1.6882826312357782]
# point1 = [715, 343] # 0.64878213
# point1_gripper = [0.5056936674804589, 0.4650883838281726, 1.0634729391460807, -0.6999271853483486, -1.749012465435611, 1.6882826312357782]
# point2 = [733, 368]
# point2_gripper = [0.5485518509025477, 0.5157901201998042, 0.9837568437877331, -0.6609672121210775, -1.7084733902118903, 1.7261963960916609]
# point1 = [733, 368] # 0.5727386
# point1_gripper = [0.5485518509025477, 0.5157901201998042, 0.9837568437877331, -0.6609672121210775, -1.7084733902118903, 1.7261963960916609]
# point2 = [738, 389]
# point2_gripper = [0.5122332451863898, 0.5128182769039566, 0.9630757531852082, -0.7115522167741687, -1.7916510038378088, 1.7013068394360287]
def pixel_normalization(point1, point2):
points_norm = []
points_norm.append(point1)
points_norm.append(point2)
points_norm = np.array(points_norm)
points_norm = (points_norm - np.mean(points_norm)) / np.std(points_norm)
return points_norm[0], points_norm[1]
if __name__ == "__main__":
p1, p2 = pixel_normalization(point1, point2)
print(p1)
print(p2)