-
Notifications
You must be signed in to change notification settings - Fork 31
/
Copy pathcam_utils.py
145 lines (108 loc) · 4.91 KB
/
cam_utils.py
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
133
134
135
136
137
138
139
140
141
142
143
144
145
# -*- coding: utf-8 -*-
# Max-Planck-Gesellschaft zur Förderung der Wissenschaften e.V. (MPG) is
# holder of all proprietary rights on this computer program.
# You can only use this computer program if you have closed
# a license agreement with MPG or you get the right to use the computer
# program from someone who is authorized to grant you that right.
# Any use of the computer program without a valid license is prohibited and
# liable to prosecution.
#
# Copyright©2019 Max-Planck-Gesellschaft zur Förderung
# der Wissenschaften e.V. (MPG). acting on behalf of its Max Planck Institute
# for Intelligent Systems. All rights reserved.
#
# Contact: ps-license@tuebingen.mpg.de
import torch
import numpy as np
from scipy.stats import norm
from pare.models.layers.softargmax import softargmax1d
def get_bins(minval, maxval, sigma, alpha, beta, kappa):
"""Remember, bin 0 = below value! last bin mean >= maxval"""
x = np.linspace(minval, maxval, 255)
rv = norm(0, sigma)
pdf = rv.pdf(x)
pdf /= (pdf.max())
pdf *= alpha
pdf = pdf.max()*beta - pdf
cumsum = np.cumsum(pdf)
cumsum = cumsum / cumsum.max() * kappa
cumsum -= cumsum[pdf.size//2]
return cumsum
pitch_bins = np.linspace(-0.6, 0.6, 255)
pitch_bins_centers = pitch_bins.copy()
pitch_bins_centers[:-1] += np.diff(pitch_bins_centers)/2
pitch_bins_centers = np.append(pitch_bins_centers, pitch_bins[-1])
horizon_bins = np.linspace(-0.5, 1.5, 255)
horizon_bins_centers = horizon_bins.copy()
horizon_bins_centers[:-1] += np.diff(horizon_bins_centers)/2
horizon_bins_centers = np.append(horizon_bins_centers, horizon_bins[-1])
roll_bins = get_bins(-np.pi/6, np.pi/6, 0.5, 0.04, 1.1, np.pi)
# roll_bins = get_bins(-np.pi/6, np.pi/6, 0.2, 0.04, 1.1, np.pi/3)
roll_bins_centers = roll_bins.copy()
roll_bins_centers[:-1] += np.diff(roll_bins_centers)/2
roll_bins_centers = np.append(roll_bins_centers, roll_bins[-1])
vfov_bins = np.linspace(0.2617, 2.1, 255)
vfov_bins_centers = vfov_bins.copy()
vfov_bins_centers[:-1] += np.diff(vfov_bins_centers)/2
vfov_bins_centers = np.append(vfov_bins_centers, vfov_bins[-1])
roll_new_bins = np.linspace(-0.6, 0.6, 255)
roll_new_bins_centers = roll_new_bins.copy()
roll_new_bins_centers[:-1] += np.diff(roll_new_bins_centers)/2
roll_new_bins_centers = np.append(roll_new_bins_centers, roll_new_bins[-1])
def bins2horizon(bins):
if isinstance(bins, torch.Tensor):
bins = bins.cpu().numpy()
idxes = np.argmax(bins, axis=bins.ndim - 1)
return horizon_bins_centers[idxes]
def bins2pitch(bins):
if isinstance(bins, torch.Tensor):
bins = bins.cpu().numpy()
idxes = np.argmax(bins, axis=bins.ndim - 1)
return pitch_bins_centers[idxes]
def bins2roll(bins):
if isinstance(bins, torch.Tensor):
bins = bins.cpu().numpy()
idxes = np.argmax(bins, axis=bins.ndim - 1)
return roll_bins_centers[idxes]
def bins2vfov(bins):
if isinstance(bins, torch.Tensor):
bins = bins.cpu().numpy()
idxes = np.argmax(bins, axis=bins.ndim - 1)
return vfov_bins_centers[idxes]
def vfov2soft_idx(angle):
return angle_to_soft_idx(angle, min=np.min(vfov_bins), max=np.max(vfov_bins))
def pitch2soft_idx(angle):
return angle_to_soft_idx(angle, min=np.min(pitch_bins), max=np.max(pitch_bins))
def roll2soft_idx(angle):
return angle_to_soft_idx(angle, min=-0.6, max=0.6)
def angle_to_soft_idx(angle, min, max):
return 2 * ((angle - min) / (max - min)) - 1
def soft_idx_to_angle(soft_idx, min, max):
return (max - min) * ((soft_idx + 1) / 2) + min
def get_softargmax(pred):
pred = pred.unsqueeze(1) # (N, 1, 256)
pred_argmax, _ = softargmax1d(pred, normalize_keypoints=True) # (N, 1, 1)
pred_argmax = pred_argmax.reshape(-1)
return pred_argmax
@torch.no_grad()
def convert_preds_to_angles(pred_vfov, pred_pitch, pred_roll, loss_type='kl', return_type='torch', legacy=False):
if loss_type in ('kl', 'ce'):
pred_vfov = bins2vfov(pred_vfov)
pred_pitch = bins2pitch(pred_pitch)
pred_roll = bins2roll(pred_roll)
elif loss_type in ('softargmax_l2', 'softargmax_biased_l2'):
pred_vfov = soft_idx_to_angle(get_softargmax(pred_vfov),
min=np.min(vfov_bins), max=np.max(vfov_bins))
pred_pitch = soft_idx_to_angle(get_softargmax(pred_pitch),
min=np.min(pitch_bins), max=np.max(pitch_bins))
if not legacy:
pred_roll = soft_idx_to_angle(get_softargmax(pred_roll), min=-0.6, max=0.6)
else:
pred_roll = bins2roll(pred_roll)
if return_type == 'np' and isinstance(pred_vfov, torch.Tensor):
return pred_vfov.cpu().numpy(), \
pred_pitch.cpu().numpy(), \
pred_roll.cpu().numpy()
if return_type == 'torch' and isinstance(pred_vfov, np.ndarray):
return torch.from_numpy(pred_vfov), torch.from_numpy(pred_pitch), torch.from_numpy(pred_roll)
return pred_vfov, pred_pitch, pred_roll