世界坐标系,相机坐标系和图像坐标系的转换(Python)

你的名字 2023-07-03 10:42 7阅读 0赞

# 世界坐标系,相机坐标系和图像坐标系的转换(Python) #

--------------------

相机内参外参说明:[https://panjinquan.blog.csdn.net/article/details/102502213][https_panjinquan.blog.csdn.net_article_details_102502213]

计算机视觉：相机成像原理：世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换:[https://blog.csdn.net/chentravelling/article/details/53558096][https_blog.csdn.net_chentravelling_article_details_53558096]

--------------------

## 1.世界坐标->相机坐标 ##

![20200205170035240.png][]

--------------------

## **2.相机坐标系->图像坐标系** ##

![这里写图片描述][SouthEast]

此时投影点p的单位还是mm，并不是pixel，需要进一步转换到像素坐标系。

## 3.图像坐标系与像素坐标系 ##

像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：其中dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm

![这里写图片描述][SouthEast 1]  
那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。   
![SouthEast 2][]

![u=\\frac\{x\}\{dx\}+u\_\{0\}=\\frac\{xf\_\{x\}\}\{Z\_\{c\}\}+u\_\{0\}][u_frac_x_dx_u_0_frac_xf_x_Z_c_u_0]

![u=\\frac\{x\}\{dx\}+u\_\{0\}=\\frac\{xf\_\{x\}\}\{Z\_\{c\}\}+u\_\{0\}][u_frac_x_dx_u_0_frac_xf_x_Z_c_u_0]![gif.latex][]

--------------------

**python代码实现:**

以下是实现变换的关键代码

相关可视化部分已经push到github:  [https://github.com/PanJinquan/python-learning-notes][https_github.com_PanJinquan_python-learning-notes]

[https://github.com/PanJinquan/python-learning-notes/blob/master/modules/utils\_3d/camera\_tools.py][https_github.com_PanJinquan_python-learning-notes_blob_master_modules_utils_3d_camera_tools.py]

# -*- coding: utf-8 -*-
    """
    # --------------------------------------------------------
    # @Project: Integral-Human-Pose-Regression-for-3D-Human-Pose-Estimation
    # @Author : panjq
    # @E-mail : pan_jinquan@163.com
    # @Date   : 2020-02-04 16:03:01
    # @url    : https://www.jianshu.com/p/c5627ad019df
    # --------------------------------------------------------
    """
    import sys
    import os
    
    sys.path.append(os.getcwd())
    
    import cv2
    import numpy as np
    from modules.utils_3d import vis_3d as vis
    from utils import image_processing
    
    human36m_camera_intrinsic = {
        # R，旋转矩阵
        "R": [[-0.91536173, 0.40180837, 0.02574754],
              [0.05154812, 0.18037357, -0.98224649],
              [-0.39931903, -0.89778361, -0.18581953]],
        # t，平移向量
        "T": [1841.10702775, 4955.28462345, 1563.4453959],
        # 焦距，f/dx, f/dy
        "f": [1145.04940459, 1143.78109572],
        # principal point，主点，主轴与像平面的交点
        "c": [512.54150496, 515.45148698]
    
    }
    
    kinect2_camera_intrinsic = {
    
        # R，旋转矩阵
        "R": [[0.999853, -0.00340388, 0.0167495],
              [0.00300206, 0.999708, 0.0239986],
              [-0.0168257, -0.0239459, 0.999571]],
        # t，平移向量
        "T": [15.2562, 70.2212, -10.9926],
        # 焦距，f/dx, f/dy
        "f": [367.535, 367.535],
        # principal point，主点，主轴与像平面的交点
        "c": [260.166, 205.197]
    
    }
    
    camera_intrinsic = human36m_camera_intrinsic
    # camera_intrinsic = kinect2_camera_intrinsic
    
    class CameraTools(object):
    
        @staticmethod
        def convert_wc_to_cc(joint_world):
            """
            世界坐标系 -> 相机坐标系: R * (pt - T):
            joint_cam = np.dot(R, (joint_world - T).T).T
            :return:
            """
            joint_world = np.asarray(joint_world)
            R = np.asarray(camera_intrinsic["R"])
            T = np.asarray(camera_intrinsic["T"])
            joint_num = len(joint_world)
            # 世界坐标系 -> 相机坐标系
            # [R|t] world coords -> camera coords
            # joint_cam = np.zeros((joint_num, 3))  # joint camera
            # for i in range(joint_num):  # joint i
            #     joint_cam[i] = np.dot(R, joint_world[i] - T)  # R * (pt - T)
            # .T is 转置, T is translation mat
            joint_cam = np.dot(R, (joint_world - T).T).T  # R * (pt - T)
            return joint_cam
    
        @staticmethod
        def convert_cc_to_wc(joint_world):
            """
            相机坐标系 -> 世界坐标系: inv(R) * pt +T 
            joint_cam = np.dot(inv(R), joint_world.T)+T
            :return:
            """
            joint_world = np.asarray(joint_world)
            R = np.asarray(camera_intrinsic["R"])
            T = np.asarray(camera_intrinsic["T"])
            # 相机坐标系 -> 世界坐标系
            joint_cam = np.dot(np.linalg.inv(R), joint_world.T).T + T
            return joint_cam
    
        @staticmethod
        def __cam2pixel(cam_coord, f, c):
            """
            相机坐标系 -> 像素坐标系: (f / dx) * (X / Z) = f * (X / Z) / dx
            cx,ppx=260.166; cy,ppy=205.197; fx=367.535; fy=367.535
            将从3D(X,Y,Z)映射到2D像素坐标P(u,v)计算公式为：
            u = X * fx / Z + cx
            v = Y * fy / Z + cy
            D(v,u) = Z / Alpha
            =====================================================
            camera_matrix = [[428.30114, 0.,   316.41648],
                            [   0.,    427.00564, 218.34591],
                            [   0.,      0.,    1.]])
    
            fx = camera_intrinsic[0, 0]
            fy = camera_intrinsic[1, 1]
            cx = camera_intrinsic[0, 2]
            cy = camera_intrinsic[1, 2]
            =====================================================
            :param cam_coord:
            :param f: [fx,fy]
            :param c: [cx,cy]
            :return:
            """
            # 等价于：(f / dx) * (X / Z) = f * (X / Z) / dx
            # 三角变换， / dx, + center_x
            u = cam_coord[..., 0] / cam_coord[..., 2] * f[0] + c[0]
            v = cam_coord[..., 1] / cam_coord[..., 2] * f[1] + c[1]
            d = cam_coord[..., 2]
            return u, v, d
    
        @staticmethod
        def convert_cc_to_ic(joint_cam):
            """
            相机坐标系 -> 像素坐标系
            :param joint_cam:
            :return:
            """
            # 相机坐标系 -> 像素坐标系，并 get relative depth
            # Subtract center depth
            # 选择 Pelvis骨盆 所在位置作为相机中心，后面用之求relative depth
            root_idx = 0
            center_cam = joint_cam[root_idx]  # (x,y,z) mm
            joint_num = len(joint_cam)
            f = camera_intrinsic["f"]
            c = camera_intrinsic["c"]
            # joint image_dict，像素坐标系，Depth 为相对深度 mm
            joint_img = np.zeros((joint_num, 3))
            joint_img[:, 0], joint_img[:, 1], joint_img[:, 2] = CameraTools.__cam2pixel(joint_cam, f, c)  # x,y
            joint_img[:, 2] = joint_img[:, 2] - center_cam[2]  # z
            return joint_img
    
    
    def demo_for_human36m():
        joint_world = [[-91.679, 154.404, 907.261],
                       [-223.23566, 163.80551, 890.5342],
                       [-188.4703, 14.077106, 475.1688],
                       [-261.84055, 186.55286, 61.438915],
                       [39.877888, 145.00247, 923.98785],
                       [-11.675994, 160.89919, 484.39148],
                       [-51.550297, 220.14624, 35.834396],
                       [-132.34781, 215.73018, 1128.8396],
                       [-97.1674, 202.34435, 1383.1466],
                       [-112.97073, 127.96946, 1477.4457],
                       [-120.03289, 190.96477, 1573.4],
                       [25.895456, 192.35947, 1296.1571],
                       [107.10581, 116.050285, 1040.5062],
                       [129.8381, -48.024918, 850.94806],
                       [-230.36955, 203.17923, 1311.9639],
                       [-315.40536, 164.55284, 1049.1747],
                       [-350.77136, 43.442127, 831.3473],
                       [-102.237045, 197.76935, 1304.0605]]
        joint_world = np.asarray(joint_world)
        # 关节点连接线
        kps_lines = ((0, 7), (7, 8), (8, 9), (9, 10), (8, 11), (11, 12), (12, 13), (8, 14), (14, 15),
                     (15, 16), (0, 1), (1, 2), (2, 3), (0, 4), (4, 5), (5, 6))
        # show in 世界坐标系
        vis.vis_3d(joint_world, kps_lines, coordinate="WC", title="WC", set_lim=True, isshow=True)
    
        kp_vis = CameraTools()
    
        # show in 相机坐标系
        joint_cam = kp_vis.convert_wc_to_cc(joint_world)
        vis.vis_3d(joint_cam, kps_lines, coordinate="CC", title="CC", set_lim=True, isshow=True)
        joint_img = kp_vis.convert_cc_to_ic(joint_cam)
    
        joint_world1 = kp_vis.convert_cc_to_wc(joint_cam)
        vis.vis_3d(joint_world1, kps_lines, coordinate="WC", title="WC", set_lim=True, isshow=True)
    
        # show in 像素坐标系
        kpt_2d = joint_img[:, 0:2]
        image_path = "./data/s_01_act_02_subact_01_ca_02_000001.jpg"
        image = image_processing.read_image(image_path)
        image = image_processing.draw_key_point_in_image(image, key_points=[kpt_2d], pointline=kps_lines)
        image_processing.cv_show_image("image_dict", image)
    
    
    if __name__ == "__main__":
        demo_for_human36m()

效果:

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d1eXVlYWxpYW4_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d1eXVlYWxpYW4_size_16_color_FFFFFF_t_70 1][]

[https_panjinquan.blog.csdn.net_article_details_102502213]: https://panjinquan.blog.csdn.net/article/details/102502213
[https_blog.csdn.net_chentravelling_article_details_53558096]: https://blog.csdn.net/chentravelling/article/details/53558096
[20200205170035240.png]: https://img-blog.csdnimg.cn/20200205170035240.png
[SouthEast]: https://img-blog.csdn.net/20161210142740999?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvY2hlbnRyYXZlbGxpbmc=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast
[SouthEast 1]: https://img-blog.csdn.net/20161210143514044?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvY2hlbnRyYXZlbGxpbmc=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast
[SouthEast 2]: https://img-blog.csdn.net/20161210144703071?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvY2hlbnRyYXZlbGxpbmc=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast
[u_frac_x_dx_u_0_frac_xf_x_Z_c_u_0]: https://private.codecogs.com/gif.latex?%5Cinline%20u%3D%5Cfrac%7Bx%7D%7Bdx%7D&plus;u_%7B0%7D%3D%5Cfrac%7Bxf_%7Bx%7D%7D%7BZ_%7Bc%7D%7D&plus;u_%7B0%7D
[gif.latex]: https://private.codecogs.com/gif.latex?
[https_github.com_PanJinquan_python-learning-notes]: https://github.com/PanJinquan/python-learning-notes
[https_github.com_PanJinquan_python-learning-notes_blob_master_modules_utils_3d_camera_tools.py]: https://github.com/PanJinquan/python-learning-notes/blob/master/modules/utils_3d/camera_tools.py
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d1eXVlYWxpYW4_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200603140449582.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d1eXVlYWxpYW4=,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d1eXVlYWxpYW4_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200603140523652.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2d1eXVlYWxpYW4=,size_16,color_FFFFFF,t_70