File size: 4,150 Bytes
88b0dcb |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 |
"""
@date: 2021/6/30
@description:
"""
import numpy as np
from typing import List
from utils.boundary import *
from scipy.optimize import least_squares
from functools import partial
def lsq_fit(ceil_norm, floor_norm):
"""
Least Squares
:param ceil_norm:
:param floor_norm:
:return:
"""
def error_fun(ratio, ceil_norm, floor_norm):
error = np.abs(ratio * ceil_norm - floor_norm)
return error
init_ratio = np.mean(floor_norm / ceil_norm, axis=-1)
error_func = partial(error_fun, ceil_norm=ceil_norm, floor_norm=floor_norm)
ret = least_squares(error_func, init_ratio, verbose=0)
ratio = ret.x[0]
return ratio
def mean_percentile_fit(ceil_norm, floor_norm, p1=25, p2=75):
"""
:param ceil_norm:
:param floor_norm:
:param p1:
:param p2:
:return:
"""
ratio = floor_norm / ceil_norm
r_min = np.percentile(ratio, p1)
r_max = np.percentile(ratio, p2)
return ratio[(r_min <= ratio) & (ratio <= r_max)].mean()
def calc_ceil_ratio(boundaries: List[np.array], mode='lsq'):
"""
:param boundaries: [ [[cu1, cv1], [cu2, cv2], ...], [[fu1, fv1], [fu2, fv2], ...] ]
:param mode: 'lsq' or 'mean'
:return:
"""
assert len(boundaries[0].shape) < 4 and len(boundaries[1].shape) < 4, 'error shape'
if not is_normal_layout(boundaries):
return 0
ceil_boundary = boundaries[0]
floor_boundary = boundaries[1]
assert ceil_boundary.shape[-2] == floor_boundary.shape[-2], "boundary need same length"
ceil_xyz = uv2xyz(ceil_boundary, -1)
floor_xyz = uv2xyz(floor_boundary, 1)
ceil_xz = ceil_xyz[..., ::2]
floor_xz = floor_xyz[..., ::2]
ceil_norm = np.linalg.norm(ceil_xz, axis=-1)
floor_norm = np.linalg.norm(floor_xz, axis=-1)
if mode == "lsq":
if len(ceil_norm.shape) == 2:
ratio = np.array([lsq_fit(ceil_norm[i], floor_norm[i]) for i in range(ceil_norm.shape[0])])
else:
ratio = lsq_fit(ceil_norm, floor_norm)
else:
if len(ceil_norm.shape) == 2:
ratio = np.array([mean_percentile_fit(ceil_norm[i], floor_norm[i]) for i in range(ceil_norm.shape[0])])
else:
ratio = mean_percentile_fit(ceil_norm, floor_norm)
return ratio
def calc_ceil_height(boundaries: List[np.array], camera_height=1.6, mode='lsq') -> float:
"""
:param boundaries: [ [[cu1, cv1], [cu2, cv2], ...], [[fu1, fv1], [fu2, fv2], ...] ]
:param camera_height:
:param mode:
:return:
"""
ratio = calc_ceil_ratio(boundaries, mode)
ceil_height = camera_height * ratio
return ceil_height
def calc_room_height(boundaries: List[np.array], camera_height=1.6, mode='lsq') -> float:
"""
:param boundaries: also can corners，format: [ [[cu1, cv1], [cu2, cv2], ...], [[fu1, fv1], [fu2, fv2], ...] ],
0 denotes ceil, 1 denotes floor
:param camera_height: actual camera height determines the scale
:param mode: fitting method lsq or mean
:return:
"""
ceil_height = calc_ceil_height(boundaries, camera_height, mode)
room_height = camera_height + ceil_height
return room_height
def height2ratio(height, camera_height=1.6):
ceil_height = height - camera_height
ratio = ceil_height / camera_height
return ratio
def ratio2height(ratio, camera_height=1.6):
ceil_height = camera_height * ratio
room_height = camera_height + ceil_height
return room_height
if __name__ == '__main__':
from dataset.mp3d_dataset import MP3DDataset
dataset = MP3DDataset(root_dir="../src/dataset/mp3d", mode="train")
for data in dataset:
ceil_corners = data['corners'][::2]
floor_corners = data['corners'][1::2]
# ceil_boundary = corners2boundary(ceil_corners, length=1024)
# floor_boundary = corners2boundary(floor_corners, length=1024)
room_height1 = calc_room_height([ceil_corners, floor_corners], camera_height=1.6, mode='mean')
room_height2 = calc_room_height([ceil_corners, floor_corners], camera_height=1.6, mode='lsq')
print(room_height1, room_height2, data['cameraCeilingHeight'] + 1.6)
|