yslan/ObjCtrl-2.5D

1import numpy as np2import plotly.express as px3import plotly.graph_objects as go4import plotly.colors as pc5 6def vis_camera(RT_list, rescale_T=1):7    fig = go.Figure()8    showticklabels = True9    visible = True10    # scene_bounds = 1.511    scene_bounds = 2.012    base_radius = 2.513    zoom_scale = 1.514    fov_deg = 50.015    16    edges = [(0, 1), (0, 2), (0, 3), (1, 2), (2, 3), (3, 1), (3, 4)] 17    18    colors = px.colors.qualitative.Plotly19    20    cone_list = []21    n = len(RT_list)22    color_scale = pc.sample_colorscale("Reds", [i / (len(RT_list) - 1) for i in range(len(RT_list))])23    # color_scale = pc.sample_colorscale("Blues ", [0.3 + 0.7 * i / (len(RT_list) - 1) for i in range(len(RT_list))])24    color_scale = pc.sample_colorscale("Blues", [0.4 + 0.6 * i / (len(RT_list) - 1) for i in range(len(RT_list))])25    # color_scale = pc.sample_colorscale("Cividis", [0.3 + 0.7 * i / (len(RT_list) - 1) for i in range(len(RT_list))])26    # color_scale = pc.sample_colorscale("Viridis", [0.3 + 0.7 * i / (len(RT_list) - 1) for i in range(len(RT_list))])27    28 29 30    for i, RT in enumerate(RT_list):31        R = RT[:,:3]32        T = RT[:,-1]/rescale_T33        cone = calc_cam_cone_pts_3d_org(R, T, fov_deg, scale=0.15)34        # cone_list.append((cone, (i*1/n, "green"), f"view_{i}"))35        # color = colors[i % len(colors)]  # 从颜色列表中循环选择颜色36        cone_list.append((cone, color_scale[i], f"view_{i}"))37 38    39    for (cone, clr, legend) in cone_list:40        for (i, edge) in enumerate(edges):41            (x1, x2) = (cone[edge[0], 0], cone[edge[1], 0])42            (y1, y2) = (cone[edge[0], 1], cone[edge[1], 1])43            (z1, z2) = (cone[edge[0], 2], cone[edge[1], 2])44            fig.add_trace(go.Scatter3d(45                x=[x1, x2], y=[y1, y2], z=[z1, z2], mode='lines',46                line=dict(color=clr, width=6),47                # line={48                #     'size': 30,49                #     'opacity': 0.8,50                # },51                name=legend, showlegend=(i == 0))) 52    fig.update_layout(53                    height=500,54                    autosize=True,55                    # hovermode=False,56                    margin=go.layout.Margin(l=0, r=0, b=0, t=0),57                    58                    showlegend=True,59                    legend=dict(60                        yanchor='bottom',61                        y=0.01,62                        xanchor='right',63                        x=0.99,64                    ),65                    scene=dict(66                        aspectmode='manual',67                        aspectratio=dict(x=1, y=1, z=1.0),68                        camera=dict(69                            center=dict(x=0.0, y=0.0, z=0.0),70                            up=dict(x=0.0, y=-1.0, z=0.0),71                            eye=dict(x=scene_bounds/2, y=-scene_bounds/2, z=-scene_bounds/2),72                            ),73 74                        xaxis=dict(75                            range=[-scene_bounds, scene_bounds],76                            showticklabels=showticklabels,77                            visible=visible,78                        ),79                            80                        81                        yaxis=dict(82                            range=[-scene_bounds, scene_bounds],83                            showticklabels=showticklabels,84                            visible=visible,85                        ),86                            87                        88                        zaxis=dict(89                            range=[-scene_bounds, scene_bounds],90                            showticklabels=showticklabels,91                            visible=visible,92                        )93                    ))94    95    return fig96 97 98def calc_cam_cone_pts_3d(R_W2C, T_W2C, fov_deg, scale=1.0, set_canonical=False, first_frame_RT=None):99    fov_rad = np.deg2rad(fov_deg)100    R_W2C_inv = np.linalg.inv(R_W2C)101 102    # 定义视锥体的长度103    height = scale  # 视锥体的高度104    width = height * np.tan(fov_rad / 2)  # 视锥体在给定FOV下的宽度105 106    # 计算相机中心位置107    T = np.zeros_like(T_W2C) - T_W2C108    T = np.dot(R_W2C_inv, T)109    cam_x, cam_y, cam_z = T110 111    # 定义视锥体的四个顶点112    corn1 = np.array([width, width, height])113    corn2 = np.array([-width, width, height])114    corn3 = np.array([-width, -width, height])115    corn4 = np.array([width, -width, height])116 117    # 将顶点从相机坐标转换到世界坐标118    corners = np.stack([corn1, corn2, corn3, corn4]) - T_W2C119    corners = np.dot(R_W2C_inv, corners.T).T120 121    # 将视锥体顶点与相机中心坐标组合122    xs = [cam_x] + corners[:, 0].tolist()123    ys = [cam_y] + corners[:, 1].tolist()124    zs = [cam_z] + corners[:, 2].tolist()125 126    return np.array([xs, ys, zs]).T127 128 129def calc_cam_cone_pts_3d_org(R_W2C, T_W2C, fov_deg, scale=0.1, set_canonical=False, first_frame_RT=None):130    fov_rad = np.deg2rad(fov_deg)131    R_W2C_inv = np.linalg.inv(R_W2C)132 133    # Camera pose center:134    T = np.zeros_like(T_W2C) - T_W2C135    T = np.dot(R_W2C_inv, T)136    cam_x = T[0]137    cam_y = T[1]138    cam_z = T[2]139    if set_canonical:140        T = np.zeros_like(T_W2C)141        T = np.dot(first_frame_RT[:,:3], T) + first_frame_RT[:,-1]142        T = T - T_W2C 143        T = np.dot(R_W2C_inv, T)144        cam_x = T[0]145        cam_y = T[1]146        cam_z = T[2]147 148    # vertex149    corn1 = np.array([np.tan(fov_rad / 2.0), 0.5*np.tan(fov_rad / 2.0), 1.0]) *scale 150    corn2 = np.array([-np.tan(fov_rad / 2.0), 0.5*np.tan(fov_rad / 2.0), 1.0]) *scale151    corn3 = np.array([0, -0.25*np.tan(fov_rad / 2.0), 1.0]) *scale152    corn4 = np.array([0, -0.5*np.tan(fov_rad / 2.0), 1.0]) *scale153 154    corn1 = corn1 - T_W2C155    corn2 = corn2 - T_W2C156    corn3 = corn3 - T_W2C157    corn4 = corn4 - T_W2C158    159    corn1 = np.dot(R_W2C_inv, corn1)160    corn2 = np.dot(R_W2C_inv, corn2)161    corn3 = np.dot(R_W2C_inv, corn3) 162    corn4 = np.dot(R_W2C_inv, corn4) 163 164    # Now attach as offset to actual 3D camera position:165    corn_x1 = corn1[0]166    corn_y1 = corn1[1]167    corn_z1 = corn1[2]168    169    corn_x2 = corn2[0]170    corn_y2 = corn2[1]171    corn_z2 = corn2[2]172    173    corn_x3 = corn3[0]174    corn_y3 = corn3[1]175    corn_z3 = corn3[2]176    177    corn_x4 = corn4[0]178    corn_y4 = corn4[1]179    corn_z4 = corn4[2]180            181 182    xs = [cam_x, corn_x1, corn_x2, corn_x3, corn_x4, ]183    ys = [cam_y, corn_y1, corn_y2, corn_y3, corn_y4, ]184    zs = [cam_z, corn_z1, corn_z2, corn_z3, corn_z4, ]185 186    return np.array([xs, ys, zs]).T187 188    189def vis_camera_rescale(RTs):190    rescale_T = 1.0191    rescale_T = max(rescale_T, np.max(np.abs(RTs[:, :, -1])) / 1.9)192    fig = vis_camera(RTs, rescale_T=rescale_T)193    # fig.show()194    return fig