File size: 6,784 Bytes
f348660 | 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 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 | import numpy as np
import time
from scipy.optimize import minimize_scalar
def compute_scale_and_shift_ls(prediction, target, mask):
# tuple specifying with axes to sum
sum_axes = (0, 1)
# system matrix: A = [[a_00, a_01], [a_10, a_11]]
a_00 = np.sum(mask * prediction * prediction, sum_axes)
a_01 = np.sum(mask * prediction, sum_axes)
a_11 = np.sum(mask, sum_axes)
# right hand side: b = [b_0, b_1]
b_0 = np.sum(mask * prediction * target, sum_axes)
b_1 = np.sum(mask * target, sum_axes)
# solution: x = A^-1 . b = [[a_11, -a_01], [-a_10, a_00]] / (a_00 * a_11 - a_01 * a_10) . b
x_0 = np.zeros_like(b_0)
x_1 = np.zeros_like(b_1)
det = a_00 * a_11 - a_01 * a_01
# A needs to be a positive definite matrix.
valid = det > 0
x_0[valid] = (a_11[valid] * b_0[valid] - a_01[valid] * b_1[valid]) / det[valid]
x_1[valid] = (-a_01[valid] * b_0[valid] + a_00[valid] * b_1[valid]) / det[valid]
return x_0, x_1
def compute_scale_and_shift_ransac(prediction, target, mask,
num_iterations, sample_size,
inlier_threshold, inlier_ratio_threshold):
# start = time.time()
best_scale = 0.0
best_shift = 0.0
best_inlier_count = 0
valid_indices = np.where(mask)
valid_count = len(valid_indices[0])
# print('valid_count: ', valid_count)
for _ in range(num_iterations):
if valid_count < sample_size:
break
# Randomly sample from valid indices
indices = np.random.choice(valid_count, size=sample_size, replace=False)
mask_sample = np.zeros_like(mask)
mask_sample[valid_indices[0][indices], valid_indices[1][indices]] = 1
# Calculate x_0 and x_1 for the sampled data
sum_axes = (0, 1)
a_00 = np.sum(mask_sample * prediction * prediction, sum_axes)
a_01 = np.sum(mask_sample * prediction, sum_axes)
a_11 = np.sum(mask_sample, sum_axes)
b_0 = np.sum(mask_sample * prediction * target, sum_axes)
b_1 = np.sum(mask_sample * target, sum_axes)
det = a_00 * a_11 - a_01 * a_01
valid = det > 0
x_0 = np.zeros_like(b_0)
x_1 = np.zeros_like(b_1)
x_0[valid] = (a_11[valid] * b_0[valid] - a_01[valid] * b_1[valid]) / det[valid]
x_1[valid] = (-a_01[valid] * b_0[valid] + a_00[valid] * b_1[valid]) / det[valid]
# Calculate residuals and count inliers
residuals = np.abs(mask * prediction * x_0 + x_1 - mask * target)
residuals = residuals[mask]
inlier_count = np.sum(residuals < inlier_threshold)
# Update best model if current model has more inliers
if inlier_count > best_inlier_count:
best_scale = x_0
best_shift = x_1
best_inlier_count = inlier_count
inlier_ratio = inlier_count / valid_count
if inlier_ratio > inlier_ratio_threshold:
break
print('best_inlier_count: ', best_inlier_count)
print('inlier_ratio: ', best_inlier_count / valid_count)
# print('best_scale: ', best_scale)
# print('best_shift: ', best_shift)
# print('time', time.time() - start)
return best_scale, best_shift
class LeastSquaresEstimator(object):
def __init__(self, estimate, target, valid):
self.estimate = estimate
self.target = target
self.valid = valid
# to be computed
self.scale = 1.0
self.shift = 0.0
self.output = None
def compute_scale_and_shift_ran(self,
num_iterations=60, sample_size=5,
inlier_threshold=0.02, inlier_ratio_threshold=0.8):
self.scale, self.shift = compute_scale_and_shift_ransac(self.estimate, self.target, self.valid,
num_iterations, sample_size,
inlier_threshold, inlier_ratio_threshold)
def compute_scale_and_shift(self):
self.scale, self.shift = compute_scale_and_shift_ls(self.estimate, self.target, self.valid)
def apply_scale_and_shift(self):
self.output = self.estimate * self.scale + self.shift
def clamp_min_max(self, clamp_min=None, clamp_max=None):
if clamp_min is not None:
if clamp_min > 0:
clamp_min_inv = 1.0/clamp_min
self.output[self.output > clamp_min_inv] = clamp_min_inv
assert np.max(self.output) <= clamp_min_inv
else: # divide by zero, so skip
pass
if clamp_max is not None:
clamp_max_inv = 1.0/clamp_max
self.output[self.output < clamp_max_inv] = clamp_max_inv
def objective_function(x_0, prediction, target, mask):
# Calculate x_0 * prediction
x_0_prediction = x_0 * prediction
# Calculate the error between x_0 * prediction and target, using the mask
error = np.sum(mask * abs(x_0_prediction - target))
return error
class Optimizer(object):
def __init__(self, estimate, target, valid, depth_type):
self.estimate = estimate
self.target = target
self.valid = valid
self.depth_type = depth_type
# to be computed
self.scale = 1.0
self.output = None
def optimize_scale(self):
if self.depth_type == 'inv':
bounds = (0.0003, 0.01)
else:
bounds = (0.5, 1.6) # pos
# Minimize the objective function using scipy.optimize.minimize_scalar
result = minimize_scalar(
objective_function, args=(self.estimate, self.target, self.valid),
bounds=bounds
)
# Extract the optimized x_0 value from the result
optimized_x_0 = result.x
self.scale = optimized_x_0
def apply_scale(self):
self.output = self.estimate * self.scale
def clamp_min_max(self, clamp_min=None, clamp_max=None):
if clamp_min is not None:
if clamp_min > 0:
clamp_min_inv = 1.0/clamp_min
self.output[self.output > clamp_min_inv] = clamp_min_inv
assert np.max(self.output) <= clamp_min_inv
else: # divide by zero, so skip
pass
if clamp_max is not None:
clamp_max_inv = 1.0/clamp_max
self.output[self.output < clamp_max_inv] = clamp_max_inv
def clamp_min_max_pos(self, clamp_min=None, clamp_max=None):
if clamp_min is not None:
if clamp_min >= 0:
self.output[self.output < clamp_min] = clamp_min
else:
pass
if clamp_max is not None:
self.output[self.output > clamp_max] = clamp_max |