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

SeventhBlue

已于 2022-08-22 15:18:16 修改

阅读量1.9k

点赞数 1

分类专栏：三维重建 Python 文章标签：算法 python numpy

于 2022-08-22 15:03:50 首次发布

本文链接：https://blog.csdn.net/qq_31112205/article/details/126466206

版权

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

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

利用的公式如下：

#!/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,

最低0.47元/天解锁文章