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

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

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

利用的公式如下：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time        :2022/7/27 14:59
# @Author      :weiz
# @ProjectName :robotVision3
# @File        :triangulation.py
# @Description :
import cv2from triangulation_data import *
import mathdef 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, Mdef 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 = 0point1_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_2def 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)

数据：

#!/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 nppoint1 = [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)

sfm算法之三角化(三角测量)相关推荐

CRC16算法之三：CRC16-CCITT-MODBUS算法的java实现
CRC16算法系列文章: CRC16算法之一:CRC16-CCITT-FALSE算法的java实现 CRC16算法之二:CRC16-CCITT-XMODEM算法的java实现 CRC16算法之三:CR ...
最大流算法之三：ISAP
最大流算法之三:ISAP <转> (2009-08-14 19:24:27) 转载▼ 标签: it 分类: 理论通常的 SAP 类算法在寻找增广路时总要先进行 BFS,BFS 的最坏情况 ...
单目三维运动重建（sfm算法）
算法介绍单目相机三维重建可以使用运动重建Structure from motion(SfM)方法,即由图像生成三维点云以及相机姿态.SfM方法的主要流程如图所示1,第一步,使用同一相机的从不同位置和 ...
五大常用算法之三：贪心算法
一.基本概念: 所谓贪心算法是指,在对问题求解时,总是做出在当前看来是最好的选择.也就是说,不从整体最优上加以考虑,他所做出的仅是在某种意义上的局部最优解. 贪心算法没有固定的算法框架,算法设计的关键 ...
NetworkX系列教程(10)-算法之三:关键路径问题
小书匠Graph图论重头戏部分来了,写到这里我感觉得仔细认真点了,可能在NetworkX中,实现某些算法就一句话的事,但是这个算法是做什么的,用在什么地方,原理是怎么样的,不清除,所以,我决定先把图 ...
常用算法之三详解 SHA1 实现（基于算法的官方原文档）及源码详细注释
写在前面在之前的工作中,用到了CRC16.MD5 和 SHA1 算法,主要用来校验下发的文件.网上关于这些算法的文章铺天盖地,以下内容仅仅是自己在学习时候的一个记录,一些套话来自于互联网.下面先 ...
【GIF动画+完整可运行源代码】C++实现插入排序——十大经典排序算法之三
十大经典排序算法系列博客-->传送门插入排序的工作原理是通过构建有序序列,对于未排序数据,在已排序序列中从后向前扫描,找到相应位置并插入. 算法步骤: 从第一个元素开始,该元素认为已经被排序: ...
五大算法之三--贪心算法
一.基本概念: 所谓贪心算法是指,在对问题求解时,总是做出在当前看来是最好的选择.也就是说,不从整体最优上加以考虑,他所做出的仅是在某种意义上的局部最优解. 贪心算法没有固定的 ...
数据结构与算法之三直接插入排序
直接插入排序的基本思想是指定输入数据的个数,从键盘输入给数组a[],从数组中第2个数a[1]与第1个数a[0]开始比较起,如果第2个数a[1]比第1个数a[0]小,则把第2个数a[1]赋给一个临时变量 ...

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

sfm算法之三角化(三角测量)相关推荐

最新文章

热门文章