测绘数据：

ShopID: facf2332619946469f4a6e0f1a2a77fa
测绘数据：http://XXXXXX/v1/api/camera/manage/getCoordConfigListByShopId?shopId=facf2332619946469f4a6e0f1a2a77fa
测绘 2D 平图：

测绘2D平面图 Scale：5 cm/pixel
工程2D平面图： 尺寸：1500*1500

生成透明 店铺 2D 平面图，并标记摄像头图位置

1. 旋转缩放测绘平面图，去匹配 工程2D平面图

在平面图上标出测绘得来的摄像头位置，和朝向，比例尺

手工匹配测绘图与工程2D平面图 透明背景PNG图，可用于OpenGL做透明纹理

2. 根据测绘摄像头数据，在工程2D地图上表示摄像头测绘数据：（像素坐标 + Scale）

2DMap.Scale：1.0677 cm / pixel
2DMap.Width = 1500 pixel
2DMap.Height = 1500 pixel
摄像头内参标定：calibW:640 calibH:480 calibFx:450 calibFy:600

Cam1: C001895 Angle: 41 Height:330 Pos: [1036,275] Lookat: [1109, 321]
Cam2: C001897 Angle: 22 Height:330 Pos: [538, 313] Lookat: [463, 347]
Cam3: C001899 Angle: 27 Height:330 Pos: [382, 614] Lookat: [467, 602]
Cam4: C001894 Angle: 26 Height:330 Pos: [1221, 279] Lookat: [,1126, 290]
Cam5: C001900 Angle: 49 Height:330 Pos: [521, 681] Lookat: [463, 752]

3.世界3D坐标系建立

鉴于OpenGL坐标系（物体、世界、照相机坐标系）属于右手坐标系:

OpenGL 是右手坐标系

我们用的OpenCV和OpenGL 用的都是右手坐标系。
所以，摄像头的世界坐标如下：

摄像头坐标系
为了方便我们计算摄像头外参，我们如下定义世界坐标系：
世界坐标系定义

1）3D世界坐标和2D平面图坐标共原点（图像的左上角）
2）3D世界坐标系 X 轴和2D平面坐标系 x 轴方向相同
3）3D世界坐标系 Y 轴和2D平面坐标系 y 轴方向相同。
4）3D世界坐标系 Z 轴相当于现实世界的高度，不过是反的，Z周朝下（应为用的是右手坐标系）所有摄像头的Z坐标都应该是负值。

4.摄像头外参

摄像头测绘数据：

cam2dCoo， cam2DShoot， camHeight， map2DScale， camTheta

摄像头位移：

Tx = cam2dCoo.X
Tz = cam2dCoo.Y
Ty = camHeight / map2DScale

摄像头朝向：

摄像头沿Cam3D.Y轴旋转角度始终为 0
摄像头沿Cam3D.X轴旋转角度：就是摄像头的俯角。
摄像头沿Cam3D.Z轴旋转角度：通过2D平面图的摄像头坐标和lookat坐标计算

举个例子：

import math
import numpy as np
from sj.ea import Ea

map2d = Ea()
map2d.width = 1500
map2d.height = 1500
map2d.scale = 1.0677

cams = Ea()
cams[1].cam_2dxy = [1036, 275]
cams[1].cam_lookat_2dxy = [1109, 321]
cams[1].pitch = 41
cams[1].cam_height = 330

cams[2].cam_2dxy = [538, 313]
cams[2].cam_lookat_2dxy = [463, 347]
cams[2].pitch = 22
cams[2].cam_height = 330

cams[3].cam_2dxy = [382, 614]
cams[3].cam_lookat_2dxy = [467, 602]
cams[3].pitch = 27
cams[3].cam_height = 330

cams[4].cam_2dxy = [1221, 279]
cams[4].cam_lookat_2dxy = [1126, 290]
cams[4].pitch = 26
cams[4].cam_height = 330

cams[5].cam_2dxy = [521, 681]
cams[5].cam_lookat_2dxy = [463, 752]
cams[5].pitch = 49
cams[5].cam_height = 330


# b从a旋转多少弧度，逆时针方向为正。
def radians(a, b):
    a = np.array(a)
    b = np.array(b)
    c = b-a
    v1 = [0, 0, 0, 1]
    v2 = [0, 0, c[0], c[1]]

    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    dx2 = v2[2] - v2[0]
    dy2 = v2[3] - v2[1]
    rad1 = math.atan2(dy1, dx1)
    rad2 = math.atan2(dy2, dx2)
    if rad1 * rad2 >= 0:
        included_angle = rad1 - rad2
    else:
        included_angle = abs(rad1) + abs(rad2)
        if included_angle > math.pi:
            included_angle = included_angle - 2*math.pi

    return included_angle

def CamRotation(cam_2dxy, cam_lookat_2dxy, pitch):
    # pitch, yaw, roll means rx, ry, rz
    roll = 0
    yaw = radians(cam_2dxy, cam_lookat_2dxy) * 180 / math.pi
    return pitch, yaw, roll

def CamTranslate(cam_2dxy, cam_height, width_2dmap, height_2dmap, scale_2dmap):
    Tx = cam_2dxy[0] / height_2dmap
    Tz = cam_2dxy[1] / height_2dmap
    scale_3d = height_2dmap * scale_2dmap
    Ty = cam_height / scale_3d
    return Tx, Ty, Tz


if __name__ == '__main__':
    for i in range(1, 6):
        print("\nCamera %d:" % i)
        Tx, Ty, Tz = CamTranslate(cams[i].cam_2dxy, cams[i].cam_height, map2d.width, map2d.height, map2d.scale)
        print("     Translate:", Tx,Ty,Tz)

        p, y, r = CamRotation(cams[i].cam_2dxy, cams[i].cam_lookat_2dxy, cams[i].pitch)
        print("     Rotation:", p,y,r)

# Output:
Camera 1:
     Translate: 0.6906666666666667 0.20605038868596046 0.18333333333333332
     Rotation: 41 57.78345394142436 0

Camera 2:
     Translate: 0.3586666666666667 0.20605038868596046 0.20866666666666667
     Rotation: 22 -65.61362889819863 0

Camera 3:
     Translate: 0.25466666666666665 0.20605038868596046 0.4093333333333333
     Rotation: 27 98.03571071053479 0

Camera 4:
     Translate: 0.814 0.20605038868596046 0.186
     Rotation: 26 -83.39516450324604 0

Camera 5:
     Translate: 0.3473333333333333 0.20605038868596046 0.454
     Rotation: 49 -39.24543466889781 0