def calculate_metrics(self): new_objectPoints = self.ObjectPoints[-1] cam = self.Camera[-1] validation_plane = self.ValidationPlane new_imagePoints = np.array(cam.project(new_objectPoints, False)) self.ImagePoints.append(new_imagePoints) #CONDITION NUMBER CALCULATION input_list = gd.extract_objectpoints_vars(new_objectPoints) input_list.append(np.array(cam.P)) mat_cond = gd.matrix_condition_number_autograd(*input_list, normalize=False) #CONDITION NUMBER WITH A NORMALIZED CALCULATION input_list = gd.extract_objectpoints_vars(new_objectPoints) input_list.append(np.array(cam.P)) mat_cond_normalized = gd.matrix_condition_number_autograd( *input_list, normalize=True) self.CondNumber.append(mat_cond) self.CondNumberNorm.append(mat_cond_normalized) ##HOMOGRAPHY ERRORS ## TRUE VALUE OF HOMOGRAPHY OBTAINED FROM CAMERA PARAMETERS Hcam = cam.homography_from_Rt() ##We add noise to the image points and calculate the noisy homography homo_dlt_error_loop = [] homo_HO_error_loop = [] homo_CV_error_loop = [] ippe_tvec_error_loop = [] ippe_rmat_error_loop = [] epnp_tvec_error_loop = [] epnp_rmat_error_loop = [] pnp_tvec_error_loop = [] pnp_rmat_error_loop = [] # WE CREATE NOISY IMAGE POINTS (BASED ON THE TRUE VALUES) AND CALCULATE # THE ERRORS WE THEN OBTAIN AN AVERAGE FOR EACH ONE for j in range(self.ValidationIters): new_imagePoints_noisy = cam.addnoise_imagePoints( new_imagePoints, mean=0, sd=self.ImageNoise) #Calculate the pose using IPPE (solution with least repro error) normalizedimagePoints = cam.get_normalized_pixel_coordinates( new_imagePoints_noisy) ippe_tvec1, ippe_rmat1, ippe_tvec2, ippe_rmat2 = pose_ippe_both( new_objectPoints, normalizedimagePoints, debug=False) ippeCam1 = cam.clone_withPose(ippe_tvec1, ippe_rmat1) #Calculate the pose using solvepnp EPNP debug = False epnp_tvec, epnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy, cam.K, debug, cv2.SOLVEPNP_EPNP, False) epnpCam = cam.clone_withPose(epnp_tvec, epnp_rmat) #Calculate the pose using solvepnp ITERATIVE pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy, cam.K, debug, cv2.SOLVEPNP_ITERATIVE, False) pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat) #Calculate errors ippe_tvec_error1, ippe_rmat_error1 = ef.calc_estimated_pose_error( cam.get_tvec(), cam.R, ippeCam1.get_tvec(), ippe_rmat1) ippe_tvec_error_loop.append(ippe_tvec_error1) ippe_rmat_error_loop.append(ippe_rmat_error1) epnp_tvec_error, epnp_rmat_error = ef.calc_estimated_pose_error( cam.get_tvec(), cam.R, epnpCam.get_tvec(), epnp_rmat) epnp_tvec_error_loop.append(epnp_tvec_error) epnp_rmat_error_loop.append(epnp_rmat_error) pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error( cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat) pnp_tvec_error_loop.append(pnp_tvec_error) pnp_rmat_error_loop.append(pnp_rmat_error) #Homography Estimation from noisy image points #DLT TRANSFORM Xo = new_objectPoints[[0, 1, 3], :] Xi = new_imagePoints_noisy Hnoisy_dlt, _, _ = homo2d.homography2d(Xo, Xi) Hnoisy_dlt = Hnoisy_dlt / Hnoisy_dlt[2, 2] #HO METHOD Xo = new_objectPoints[[0, 1, 3], :] Xi = new_imagePoints_noisy Hnoisy_HO = hh(Xo, Xi) #OpenCV METHOD Xo = new_objectPoints[[0, 1, 3], :] Xi = new_imagePoints_noisy Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2), Xi[:2].T.reshape(1, -1, 2)) ## ERRORS FOR THE DLT HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0) Xi = np.copy(validation_imagePoints) homo_dlt_error_loop.append( ef.validation_points_error(Xi, Xo, Hnoisy_dlt)) ## ERRORS FOR THE HO HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0) Xi = np.copy(validation_imagePoints) homo_HO_error_loop.append( ef.validation_points_error(Xi, Xo, Hnoisy_HO)) ## ERRORS FOR THE OpenCV HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0) Xi = np.copy(validation_imagePoints) homo_CV_error_loop.append( ef.validation_points_error(Xi, Xo, Hnoisy_OpenCV)) self.Homo_DLT_mean.append(np.mean(homo_dlt_error_loop)) self.Homo_HO_mean.append(np.mean(homo_HO_error_loop)) self.Homo_CV_mean.append(np.mean(homo_CV_error_loop)) self.ippe_tvec_error_mean.append(np.mean(ippe_tvec_error_loop)) self.ippe_rmat_error_mean.append(np.mean(ippe_rmat_error_loop)) self.epnp_tvec_error_mean.append(np.mean(epnp_tvec_error_loop)) self.epnp_rmat_error_mean.append(np.mean(epnp_rmat_error_loop)) self.pnp_tvec_error_mean.append(np.mean(pnp_tvec_error_loop)) self.pnp_rmat_error_mean.append(np.mean(pnp_rmat_error_loop)) self.Homo_DLT_std.append(np.std(homo_dlt_error_loop)) self.Homo_HO_std.append(np.std(homo_HO_error_loop)) self.Homo_CV_std.append(np.std(homo_CV_error_loop)) self.ippe_tvec_error_std.append(np.std(ippe_tvec_error_loop)) self.ippe_rmat_error_std.append(np.std(ippe_rmat_error_loop)) self.epnp_tvec_error_std.append(np.std(epnp_tvec_error_loop)) self.epnp_rmat_error_std.append(np.std(epnp_rmat_error_loop)) self.pnp_tvec_error_std.append(np.std(pnp_tvec_error_loop)) self.pnp_rmat_error_std.append(np.std(pnp_rmat_error_loop))
def decision_function(self, X): X = check_array(X) if not self.train_sensitive_cols: X = np.delete(X, self.sensitive_col_idx_, axis=1) return super().decision_function(X)
# One Hot encode the class labels encoder = OneHotEncoder(sparse=False) y = encoder.fit_transform(y_) # # Split the data for training and testing # train_x, test_x, train_y, test_y = train_test_split(x, y, test_size=0.20) initial_weights = np.genfromtxt(fname='inputs/SF5d_5.dat') #initial_weights = np.genfromtxt(fname='inputs/keras_inputs.txt') train_x = np.genfromtxt(fname='same_split/train_x.txt') train_y = np.genfromtxt(fname='same_split/train_y.txt') test_x = np.genfromtxt(fname='same_split/test_x.txt') test_y = np.genfromtxt(fname='same_split/test_y.txt') train_x = np.delete(np.delete(train_x, 0, 1), 0, 1) test_x = np.delete(np.delete(test_x, 0, 1), 0, 1) x = np.delete(np.delete(x, 0, 1), 0, 1) #Standardization train_x[:, 0] = (train_x[:, 0] - np.mean(train_x[:, 0])) / np.std(train_x[:, 0]) train_x[:, 1] = (train_x[:, 1] - np.mean(train_x[:, 1])) / np.std(train_x[:, 1]) test_x[:, 0] = (test_x[:, 0] - np.mean(test_x[:, 0])) / np.std(test_x[:, 0]) test_x[:, 1] = (test_x[:, 1] - np.mean(test_x[:, 1])) / np.std(test_x[:, 1]) for out in range(1): acc = np.array([]) err = np.array([])
def read_data(filename): data = np.genfromtxt(filename, dtype=float, delimiter=',', skip_header=1) np.random.shuffle(data) return np.delete(data, [0], axis = 1), data[:,[0]]
def region_coloring(self, region, ax): #### color first regions #### # generate input range for functions minx = min(min(self.x[:, 0]), min(self.x[:, 1])) maxx = max(max(self.x[:, 0]), max(self.x[:, 1])) gapx = (maxx - minx) * 0.1 minx -= gapx maxx += gapx # plot over range r = np.linspace(minx, maxx, 200) x1_vals, x2_vals = np.meshgrid(r, r) x1_vals.shape = (len(r)**2, 1) x2_vals.shape = (len(r)**2, 1) o = np.ones((len(r)**2, 1)) x = np.concatenate([o, x1_vals, x2_vals], axis=1) ### for region 1, determine points that are uniquely positive for each classifier ### ind_set = [] y = np.dot(self.W, x.T) num_classes = np.size(np.unique(self.y)) if region == 1 or region == 'all': for i in range(0, num_classes): class_inds = np.arange(num_classes) class_inds = np.delete(class_inds, (i), axis=0) # loop over non-current classifier ind = np.argwhere(y[class_inds[0]] < 0).tolist() ind = [s[0] for s in ind] for j in range(1, len(class_inds)): c_ind = class_inds[j] ind2 = np.argwhere(y[c_ind] < 0).tolist() ind2 = [s[0] for s in ind2] ind = [s for s in ind if s in ind2] ind2 = np.argwhere(y[i] > 0).tolist() ind2 = [s[0] for s in ind2] ind = [s for s in ind if s in ind2] # plot polygon over region defined by ind x1_ins = np.asarray([x1_vals[s] for s in ind]) x1_ins.shape = (len(x1_ins), 1) x2_ins = np.asarray([x2_vals[s] for s in ind]) x2_ins.shape = (len(x2_ins), 1) h = np.concatenate((x1_ins, x2_ins), axis=1) vertices = ConvexHull(h).vertices poly = [h[v] for v in vertices] polygon = Polygon(poly, True) patches = [] patches.append(polygon) p = PatchCollection(patches, alpha=0.2, color=self.colors[i]) ax.add_collection(p) if region == 2 or region == 'all': for i in range(0, num_classes): class_inds = np.arange(num_classes) class_inds = np.delete(class_inds, (i), axis=0) # loop over non-current classifier ind = np.argwhere(y[class_inds[0]] > 0).tolist() ind = [s[0] for s in ind] for j in range(1, len(class_inds)): c_ind = class_inds[j] ind2 = np.argwhere(y[c_ind] > 0).tolist() ind2 = [s[0] for s in ind2] ind = [s for s in ind if s in ind2] ind2 = np.argwhere(y[i] < 0).tolist() ind2 = [s[0] for s in ind2] ind = [s for s in ind if s in ind2] # plot polygon over region defined by ind x1_ins = np.asarray([x1_vals[s] for s in ind]) x1_ins.shape = (len(x1_ins), 1) x2_ins = np.asarray([x2_vals[s] for s in ind]) x2_ins.shape = (len(x2_ins), 1) o = np.ones((len(x2_ins), 1)) h = np.concatenate((o, x1_ins, x2_ins), axis=1) # determine regions dominated by one classifier or the other vals = [] for c in class_inds: w = self.W[int(c)] nv = np.dot(w, h.T) vals.append(nv) vals = np.asarray(vals) vals.shape = (len(class_inds), len(h)) ind = np.argmax(vals, axis=0) for j in range(len(class_inds)): # make polygon for each subregion ind1 = np.argwhere(ind == j) x1_ins2 = np.asarray([x1_ins[s] for s in ind1]) x1_ins2.shape = (len(x1_ins2), 1) x2_ins2 = np.asarray([x2_ins[s] for s in ind1]) x2_ins2.shape = (len(x2_ins2), 1) h = np.concatenate((x1_ins2, x2_ins2), axis=1) # find convex hull of points vertices = ConvexHull(h).vertices poly = [h[v] for v in vertices] polygon = Polygon(poly, True) patches = [] patches.append(polygon) c = class_inds[j] p = PatchCollection(patches, alpha=0.2, color=self.colors[c]) ax.add_collection(p) if region == 3 or region == 'all': # find negative zone of all classifiers ind = np.argwhere(y[0] < 0).tolist() ind = [s[0] for s in ind] for i in range(1, num_classes): ind2 = np.argwhere(y[i] < 0).tolist() ind2 = [s[0] for s in ind2] ind = [s for s in ind if s in ind2] # loop over negative zone, find max area of each classifier x1_ins = np.asarray([x1_vals[s] for s in ind]) x1_ins.shape = (len(x1_ins), 1) x2_ins = np.asarray([x2_vals[s] for s in ind]) x2_ins.shape = (len(x2_ins), 1) o = np.ones((len(x2_ins), 1)) h = np.concatenate((o, x1_ins, x2_ins), axis=1) # determine regions dominated by one classifier or the other vals = [] for c in range(num_classes): w = self.W[c] nv = np.dot(w, h.T) vals.append(nv) vals = np.asarray(vals) vals.shape = (num_classes, len(h)) ind = np.argmax(vals, axis=0) # loop over each class, construct polygon region for each for c in range(num_classes): # make polygon for each subregion ind1 = np.argwhere(ind == c) x1_ins2 = np.asarray([x1_ins[s] for s in ind1]) x1_ins2.shape = (len(x1_ins2), 1) x2_ins2 = np.asarray([x2_ins[s] for s in ind1]) x2_ins2.shape = (len(x2_ins2), 1) h = np.concatenate((x1_ins2, x2_ins2), axis=1) # find convex hull of points vertices = ConvexHull(h).vertices poly = [h[v] for v in vertices] polygon = Polygon(poly, True) patches = [] patches.append(polygon) p = PatchCollection(patches, alpha=0.2, color=self.colors[c]) ax.add_collection(p)
def EarlyStopping( input_, target, validation_runs, initial_scales, initial_nuggets, input_pipe, output_pipe, scales_rate=0.001, nuggets_rate=0.003, nsteps=1500, ): valid_par = input_[validation_runs.tolist()] valid_obs = target[validation_runs.tolist()] input_ = np.delete(input_, validation_runs, 0) target = np.delete(target, validation_runs, 0) input_pipe.Fit(input_) output_pipe.Fit(target) valid_par = input_pipe.Transform(valid_par) valid_obs = output_pipe.Transform(valid_obs) emulator = EmulatorMultiOutput(input_pipe.Transform(input_), output_pipe.Transform(target)) emulator.SetCovariance(squared_exponential) scales_list = [] nuggets_list = [] for index, emu in enumerate(emulator.emulator_list): gd = GradientDescentForEmulator(scales_rate, nuggets_rate) gd.SetFunc(emu.MarginalLikelihood) nuggets = initial_nuggets scales = initial_scales hist_scales = [] hist_nuggets = [] partial_likelihood = [] for i in range(nsteps): scales, nuggets, grad_scales, grad_nuggets = gd.StepDescent( scales, nuggets) emu.SetScales(scales) emu.SetNuggets(nuggets) emu.StartUp() hist_scales.append(scales) hist_nuggets.append(nuggets) val = 0 for par, exp in zip(valid_par, valid_obs): val += emu.LogLikelihood(par.reshape(1, -1), exp[index].reshape(1, -1)) partial_likelihood.append(val) sys.stdout.write( "\rProcessing %i iteration, likelihood = %f, nuggets = %f, scales = %s" % (i, val, nuggets, np.array2string(scales))) sys.stdout.flush() # plt.plot(partial_likelihood) # plt.show() # get index corresponds to maximum validation log likelihood i, value = max(enumerate(partial_likelihood), key=operator.itemgetter(1)) print("max", i) scales_list.append(hist_scales[i]) nuggets_list.append(hist_nuggets[i]) return np.array(scales_list), np.array(nuggets_list)
Xi = new_imagePoints_noisy Hnoisy_HO = hh(Xo, Xi) #OpenCV METHOD Xo = new_objectPoints[[0, 1, 3], :] Xi = new_imagePoints_noisy Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2), Xi[:2].T.reshape(1, -1, 2)) ## ERRORS FOR THE DLT HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0) Xi = np.copy(validation_imagePoints) homo_dlt_error_loop.append( ef.validation_points_error(Xi, Xo, Hnoisy_dlt)) ## ERRORS FOR THE HO HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0) Xi = np.copy(validation_imagePoints) homo_HO_error_loop.append( ef.validation_points_error(Xi, Xo, Hnoisy_HO))
def apply_poly(self, x_poly, lst_poly): res = Poly() k_h, k_w = self.kernel lw, up = x_poly.lw.copy(), x_poly.up.copy() lw = lw.reshape(x_poly.shape) up = up.reshape(x_poly.shape) p = self.padding lw_pad = np.pad(lw, ((0, 0), (0, 0), (p, p), (p, p)), mode='constant') up_pad = np.pad(up, ((0, 0), (0, 0), (p, p), (p, p)), mode='constant') x_n, x_c, x_h, x_w = lw_pad.shape res_h = int((x_h - k_h) / self.stride) + 1 res_w = int((x_w - k_w) / self.stride) + 1 len_pad = x_c * x_h * x_w len_res = x_c * res_h * res_w c_idx, h_idx, w_idx = index2d(x_c, self.stride, self.kernel, (x_h, x_w)) res_lw, res_up = [], [] mx_lw_idx_lst, mx_up_idx_lst = [], [] for c in range(x_c): for i in range(res_h * res_w): mx_lw_val, mx_lw_idx = -1e9, None for k in range(k_h * k_w): h, w = h_idx[k, i], w_idx[k, i] val = lw_pad[0, c, h, w] if val > mx_lw_val: mx_lw_val, mx_lw_idx = val, (c, h, w) mx_up_val, cnt = -1e9, 0 for k in range(k_h * k_w): h, w = h_idx[k, i], w_idx[k, i] val = up_pad[0, c, h, w] if val > mx_up_val: mx_up_val = val if mx_lw_idx != (c, h, w) and val > mx_lw_val: cnt += 1 res_lw.append(mx_lw_val) res_up.append(mx_up_val) mx_lw_idx_lst.append(mx_lw_idx) if cnt > 0: mx_up_idx_lst.append(None) else: mx_up_idx_lst.append(mx_lw_idx) res.lw = np.array(res_lw) res.up = np.array(res_up) res.le = np.zeros([len_res, len_pad + 1]) res.ge = np.zeros([len_res, len_pad + 1]) res.shape = (1, x_c, res_h, res_w) for i in range(len_res): c = mx_lw_idx_lst[i][0] h = mx_lw_idx_lst[i][1] w = mx_lw_idx_lst[i][2] idx = c * x_h * x_w + h * x_w + w res.ge[i, idx] = 1 if mx_up_idx_lst[i] is None: res.le[i, -1] = res.up[i] else: res.le[i, idx] = 1 del_idx = [] if self.padding > 0: del_idx = del_idx + list(range(self.padding * (x_w + 1))) mx = x_h - self.padding for i in range(self.padding + 1, mx): tmp = i * x_w del_idx = del_idx + list( range(tmp - self.padding, tmp + self.padding)) del_idx = del_idx + list(range(mx * x_h - self.padding, x_h * x_w)) tmp = np.array(del_idx) for i in range(1, x_c): offset = i * x_h * x_w del_idx = del_idx + list((tmp + offset).copy()) res.le = np.delete(res.le, del_idx, 1) res.ge = np.delete(res.ge, del_idx, 1) return res
def apply_poly(self, x_poly, lst_poly): res = Poly() f_n, f_c, f_h, f_w = self.filters.shape x_n, x_c, x_h, x_w = x_poly.shape x_w += 2 * self.padding x_h += 2 * self.padding res_h = int((x_h - f_h) / self.stride) + 1 res_w = int((x_w - f_w) / self.stride) + 1 len_pad = x_c * x_h * x_w len_res = f_n * res_h * res_w res.lw = np.zeros(len_res) res.up = np.zeros(len_res) res.le = np.zeros([len_res, len_pad + 1]) res.ge = np.zeros([len_res, len_pad + 1]) res.shape = (1, f_n, res_h, res_w) for i in range(f_n): base = np.zeros([x_c, x_h, x_w]) base[:f_c, :f_h, :f_w] = self.filters[i] base = np.reshape(base, -1) w_idx = f_w for j in range(res_h * res_w): res.le[i * res_h * res_w + j] = np.append(base, [self.bias[i]]) res.ge[i * res_h * res_w + j] = np.append(base, [self.bias[i]]) if w_idx + self.stride <= x_w: base = np.roll(base, self.stride) w_idx += self.stride else: base = np.roll(base, self.stride * x_w - w_idx + f_w) w_idx = f_w del_idx = [] if self.padding > 0: del_idx = del_idx + list(range(self.padding * (x_w + 1))) mx = x_h - self.padding for i in range(self.padding + 1, mx): tmp = i * x_w del_idx = del_idx + list( range(tmp - self.padding, tmp + self.padding)) del_idx = del_idx + list(range(mx * x_w - self.padding, x_h * x_w)) tmp = np.array(del_idx) for i in range(1, x_c): offset = i * x_h * x_w del_idx = del_idx + list((tmp + offset).copy()) res.le = np.delete(res.le, del_idx, 1) res.ge = np.delete(res.ge, del_idx, 1) res.back_substitute(lst_poly) return res
def sliding_window_opt(): xy_cor_list = [] xy_cor_list_opt = [] xy_cor_list_gt = [] distance_error_list = [] diff_score_list = [] deep_net = dlk.custom_net(model_path) # opt_img_height参数决定了实时图与基准图之间的分辨率大小 I_org, GPS_list = img_utility.load_I(image_dir, image_dir_ext, opt_img_height) _, img_c, img_h, img_w = I_org.shape M = img_utility.load_M(map_loc) _, map_h, map_w = M.shape # 这里的P参数是需要我们传进来的 P_init_org, P_org = pama_utility.load_P(motion_param_loc) P_opt = np.zeros(P_org.shape) curr_P_init = P_init_org # 就是离线跑的,人为这里的循环次数就是P_org的长度 for i in range(P_org.shape[0]): start_timestamp = time.clock() I = I_org[i:i + 2, :, :, :] # t,V是之后优化中会用到的参数,这里进行定义 # templates indices T = np.array([1]) # visibility neighborhood V = np.array([np.arange(50), np.array([0])]) V_ = np.array([np.array([0])]) V = np.delete(V, 0) # curr_P shape [1,8,1] curr_P = np.expand_dims(P_org[i, :, :], axis=0) # 这里的改变是让整个矩阵的转移大小可以符合我们的要求 curr_P = pama_utility.scale_P(curr_P, s=scale_img_to_map) P_mk = compute_Pmk(curr_P_init, curr_P, T) # 采用瀑布流的形式(从粗到细,进行结果的细化 for idx in range(2): # 从基准地图中获取信息的步骤 # 同时增加一个参数,接收其坐标值 M_tmpl, xy_cor = extract_map_templates(P_mk, M, T, img_c, img_h, img_w) xy_cor_list.append(xy_cor) ### # 提取得到M图中的patch之后,再结合实时图(tamplate frame)去提取两者之间的特征,进而进一步进行单应矩阵的细调 T_np = np.expand_dims(I[1, :, :, :], axis=0) T_tens = Variable(torch.from_numpy(T_np).float()) T_tens_nmlz = dlk.normalize_img_batch(T_tens) T_feat_tens = deep_net(T_tens_nmlz) T_feat = T_feat_tens.data.numpy() M_tmpl_tens = Variable(torch.from_numpy(M_tmpl).float()) M_tmpl_tens_nmlz = dlk.normalize_img_batch(M_tmpl_tens) M_feat_tens = deep_net(M_tmpl_tens_nmlz) M_feat = M_feat_tens.data.numpy() ### # 使用dlk的纠正手段 dlk_net = dlk.DeepLK(dlk.custom_net(model_path)) p_lk, _, itr_dlk = dlk_net(M_tmpl_tens_nmlz, T_tens_nmlz, tol=1e-4, max_itr=max_itr_dlk, conv_flag=1, ret_itr=True) # 计算patch与实时图之间的差距 diff_score = torch.sqrt( torch.sum(torch.pow(M_tmpl_tens_nmlz - T_tens_nmlz, 2))).item() diff_score = diff_score / (M_tmpl_tens.shape[1] * M_tmpl_tens.shape[2] * M_tmpl_tens.shape[3]) print("基准子图与实时图之间的差距为:", diff_score) diff_score_list.append(diff_score) p_lk = p_lk.cpu() p_lk2x = pama_utility.scale_P(p_lk.data.numpy(), scaling_for_disp) # 只使用VO的结果 # p_lk2x = np.zeros([1, 8, 1]) s_sm = (scaling_for_disp * opt_img_height) / map_h curr_P_init_scale = pama_utility.scale_P(curr_P_init, s_sm) curr_P_scale = pama_utility.scale_P(curr_P, s_sm) H_rel_samp = pama_utility.p_to_H(p_lk2x) H_org_samp = pama_utility.p_to_H(curr_P_scale) H_rel_coord = np.linalg.inv(H_rel_samp) H_org_coord = np.linalg.inv(H_org_samp) H_opt_coord = H_rel_coord @ H_org_coord H_opt_samp = np.linalg.inv(H_opt_coord) P_opt_i_scale = pama_utility.H_to_p(H_opt_samp) s_lg = map_h / (scaling_for_disp * opt_img_height) P_opt_i = pama_utility.scale_P(P_opt_i_scale, s_lg) P_opt[i, :, :] = P_opt_i P_mk_opt = compute_Pmk(curr_P_init_scale, P_opt_i_scale, T) P_mk0 = compute_Pmk(curr_P_init_scale, curr_P_scale, T) P_mk_opt_map = pama_utility.scale_P(P_mk_opt, s_lg) P_mk0_map = pama_utility.scale_P(P_mk0, s_lg) H_mk_samp = pama_utility.p_to_H(P_mk_opt) H_mk0_samp = pama_utility.p_to_H(P_mk0) H_mk = np.linalg.inv(H_mk_samp) H_mk0 = np.linalg.inv(H_mk0_samp) H_mk_rel = np.matmul(H_mk, np.linalg.inv(H_mk0)) H_mk_rel_samp = np.linalg.inv(H_mk_rel) P_mk_rel_samp = pama_utility.H_to_p(H_mk_rel_samp) H_init_samp = pama_utility.p_to_H(curr_P_init) H_opt_i_samp = pama_utility.p_to_H(P_opt_i) H_init_coord = np.linalg.inv(H_init_samp) H_opt_i_coord = np.linalg.inv(H_opt_i_samp) H_init_coord_new = H_opt_i_coord @ H_init_coord H_init_samp_new = np.linalg.inv(H_init_coord_new) curr_P_init = pama_utility.H_to_p(H_init_samp_new) P_mk = curr_P_init curr_P = torch.zeros([1, 8, 1]) # need to rescale P_init_org and P_org to opt_img_height, from map_h # s_sm = opt_img_height / map_h; # s_sm = (scaling_for_disp * opt_img_height) / map_h ######################################### # curr_P_init_scale = pama_utility.scale_P(curr_P_init, s_sm) # curr_P_scale = pama_utility.scale_P(curr_P, s_sm) # # # 这部分的内容是在增加优化部分的内容 # # 直接注释掉这一行就是不使用优化跑的结果 # # P_opt_i_scale = optimize_wmap(I, curr_P_scale, T, V, curr_P_init_scale, M_feat, T_feat, tol, max_itr, lam1, lam2) # # curr_P_scale = optimize_wmap(I_2x, curr_P_scale, T, V, curr_P_init_scale, M_feat_2x, T_feat_2x, tol, max_itr, lam1, lam2) # # H_rel_samp = pama_utility.p_to_H(p_lk2x) # H_org_samp = pama_utility.p_to_H(curr_P_scale) # H_rel_coord = np.linalg.inv(H_rel_samp) # H_org_coord = np.linalg.inv(H_org_samp) # H_opt_coord = H_rel_coord @ H_org_coord # H_opt_samp = np.linalg.inv(H_opt_coord) # # P_opt_i_scale = pama_utility.H_to_p(H_opt_samp) # s_lg = map_h / (scaling_for_disp * opt_img_height) # P_opt_i = pama_utility.scale_P(P_opt_i_scale, s_lg) # P_opt[i, :, :] = P_opt_i # # ### plotting # P_mk_opt = compute_Pmk(curr_P_init_scale, P_opt_i_scale, T) # P_mk0 = compute_Pmk(curr_P_init_scale, curr_P_scale, T) # P_mk_opt_map = pama_utility.scale_P(P_mk_opt, s_lg) # P_mk0_map = pama_utility.scale_P(P_mk0, s_lg) # H_mk_samp = pama_utility.p_to_H(P_mk_opt) # H_mk0_samp = pama_utility.p_to_H(P_mk0) # # # invert sampling params to get coord params: # H_mk = np.linalg.inv(H_mk_samp) # H_mk0 = np.linalg.inv(H_mk0_samp) # # # compute relative hmg: # H_mk_rel = np.matmul(H_mk, np.linalg.inv(H_mk0)) # # # invert back to sampling hmg: # H_mk_rel_samp = np.linalg.inv(H_mk_rel) # # # convert sampling hmg back to sampling params: # P_mk_rel_samp = pama_utility.H_to_p(H_mk_rel_samp) # # # # compose new P_opt_i with curr_P_init # H_init_samp = pama_utility.p_to_H(curr_P_init) # H_opt_i_samp = pama_utility.p_to_H(P_opt_i) # # H_init_coord = np.linalg.inv(H_init_samp) # H_opt_i_coord = np.linalg.inv(H_opt_i_samp) # # H_init_coord_new = H_opt_i_coord @ H_init_coord # # H_init_samp_new = np.linalg.inv(H_init_coord_new) # # curr_P_init = pama_utility.H_to_p(H_init_samp_new) end_timestamp = time.clock() print("一张图片定位所需时间为:", end_timestamp - start_timestamp) print('finished iteration: {:d}'.format(i + 1)) # 定位过程结束,以下部分是画图部分的内容 side_margin = 0.15 top_margin = 0.15 targ_box = np.array([ [ M_tmpl_tens.shape[3] * side_margin, M_tmpl_tens.shape[2] * top_margin ], [ M_tmpl_tens.shape[3] * (1 - side_margin), M_tmpl_tens.shape[2] * top_margin ], [ M_tmpl_tens.shape[3] * (1 - side_margin), M_tmpl_tens.shape[2] * (1 - side_margin) ], [ M_tmpl_tens.shape[3] * side_margin, M_tmpl_tens.shape[2] * (1 - side_margin) ], [ M_tmpl_tens.shape[3] * side_margin, M_tmpl_tens.shape[2] * side_margin ], [ M_tmpl_tens.shape[3] * (1 - side_margin), M_tmpl_tens.shape[2] * (1 - side_margin) ], ]) plt.subplot(3, 1, 1) plt.title('M{:d}'.format(i + 1)) plt.imshow(img_utility.plt_axis_match_np(M_tmpl_tens[0, :, :, :])) plt.plot(targ_box[:, 0], targ_box[:, 1], 'r-') plt.plot(round(M_tmpl_tens.shape[3] / 2), round(M_tmpl_tens.shape[2] / 2), 'ro') plt.axis('off') plt.subplot(3, 1, 2) plt.title('M{:d} Warp'.format(i + 1)) M_tmpl_curr_tens = M_tmpl_tens.float() P_mk_rel_samp_curr = torch.from_numpy( pama_utility.scale_P(P_mk_rel_samp, scaling_for_disp)).float() M_tmpl_w, _, xy_cor_curr_opt = dlk.warp_hmg(M_tmpl_curr_tens, P_mk_rel_samp_curr) # 进一步优化的内容 # xy_cor_opt变量中存的是新的细化后的图像在原图像(上一次warp+crop之后的图像)中坐标位置 print("未经过优化的绝对坐标为:", xy_cor) # print("经过优化的相对的坐标为:", xy_cor_curr_opt) xy_patch_org_cor_opt = dlk.warp_hmg_Noncentric(M, P_mk, xy_cor_curr_opt, img_w=T_tens.shape[3], img_h=T_tens.shape[2]) xy_cor_list_opt.append(xy_patch_org_cor_opt) print("经过优化的绝对的坐标为:", xy_patch_org_cor_opt) xy_cor_curr_gt = srh.lag_log_to_pix_pos(M, GPS_list[i + 1]) xy_cor_list_gt.append(xy_cor_curr_gt) print("实际坐标为:", [xy_cor_curr_gt[0], xy_cor_curr_gt[1]]) distance_error_curr = map_resolution * sqrt( (xy_cor_curr_gt[0] - xy_patch_org_cor_opt[0])**2 + (xy_cor_curr_gt[1] - xy_patch_org_cor_opt[1])**2) print("实际坐标与定位结果之间的距离:", distance_error_curr, "m") distance_error_list.append(distance_error_curr) plt.imshow(img_utility.plt_axis_match_tens(M_tmpl_w[0, :, :, :])) # plt.plot(M_tmpl_w.shape[3]/2, M_tmpl_w.shape[2]/2, 'ro') plt.plot(targ_box[:, 0], targ_box[:, 1], 'r-') plt.plot(round(M_tmpl_tens.shape[3] / 2), round(M_tmpl_tens.shape[2] / 2), 'ro') plt.axis('off') plt.subplot(3, 1, 3) plt.imshow(img_utility.plt_axis_match_np(I_org[i + 1, :, :, :])) # plt.plot(I_org_2x.shape[3]/2, I_org_2x.shape[2]/2, 'ro') plt.plot(targ_box[:, 0], targ_box[:, 1], 'r-') plt.plot(round(M_tmpl_tens.shape[3] / 2), round(M_tmpl_tens.shape[2] / 2), 'ro') plt.title('I{:d}'.format(i + 1)) plt.axis('off') # 可以先不显示这里每一步的warp过程 plt.show() # # 画一下优化对比的结果 # # 这一步是比较耗时的一步,基本上去掉这一步之后可以达到实时运行的效果了 # if i == 60: # plt.subplot(2, 1, 1) # M_pil_marked = srh.Add_M_Marker(transforms.ToPILImage()(torch.from_numpy(M)), xy_patch_org_cor_opt, # color="blue") # M_pil_marked = srh.Add_M_Marker(M_pil_marked, xy_cor, color="red") # M_pil_marked = srh.Add_M_Marker(M_pil_marked, xy_cor_curr_gt, color="green") # plt.title('当前定位结果,优化前:红;优化后:蓝;实际点:绿') # plt.imshow(M_pil_marked) # # plt.subplot(2, 1, 2) # M_pil_marked = srh.Add_M_Markers_list(M, xy_cor_list, color="red") # M_pil_marked = srh.Add_M_Markers_list(M_pil_marked, xy_cor_list_opt, color="blue") # M_pil_marked = srh.Add_M_Markers_list(M_pil_marked, xy_cor_list_gt, color="green") # plt.title('飞行轨迹历史,优化前:红;优化后:蓝;实际点:绿') # plt.imshow(M_pil_marked) # plt.show() # # plt.plot(diff_score_list) # plt.show() plt.subplot(2, 1, 1) M_pil_marked = srh.Add_M_Marker(transforms.ToPILImage()( torch.from_numpy(M)), xy_patch_org_cor_opt, color="blue") M_pil_marked = srh.Add_M_Marker(M_pil_marked, xy_cor, color="red") M_pil_marked = srh.Add_M_Marker(M_pil_marked, xy_cor_curr_gt, color="green") plt.title('当前定位结果,优化前:红;优化后:蓝;实际点:绿') plt.imshow(M_pil_marked) plt.subplot(2, 1, 2) M_pil_marked = srh.Add_M_Markers_list(M, xy_cor_list, color="red") M_pil_marked = srh.Add_M_Markers_list(M_pil_marked, xy_cor_list_opt, color="blue") M_pil_marked = srh.Add_M_Markers_list(M_pil_marked, xy_cor_list_gt, color="green") plt.title('飞行轨迹历史,优化前:红;优化后:蓝;实际点:绿') plt.imshow(M_pil_marked) plt.show() plt.plot(diff_score_list) plt.show() plt.plot(distance_error_list) plt.show() M_pil_marked.save('marked.png') draw = ImageDraw.Draw(M_pil_marked) for i in range(len(xy_cor_list_opt) - 1): draw.line((xy_cor_list_opt[i][0], xy_cor_list_opt[i][1], xy_cor_list_opt[i + 1][0], xy_cor_list_opt[i + 1][1]), fill='blue', width=5) draw.line((xy_cor_list[i][0], xy_cor_list[i][1], xy_cor_list[i + 1][0], xy_cor_list[i + 1][1]), fill='red', width=5) draw.line((xy_cor_list_gt[i][0], xy_cor_list_gt[i][1], xy_cor_list_gt[i + 1][0], xy_cor_list_gt[i + 1][1]), fill='green', width=5) M_pil_marked.save('marked_line.png') # s_rel_pose = float(img_h_rel_pose) / map_h s_rel_pose = 1 P_opt_scale = pama_utility.scale_P(P_opt, s_rel_pose) H_opt_rel_samp = pama_utility.p_to_H(P_opt_scale) H_opt_rel_coord = np.linalg.inv(H_opt_rel_samp) P_opt_map_scale_coord = pama_utility.H_to_p(H_opt_rel_coord) P_opt_map_scale_coord_sqz = np.squeeze(P_opt_map_scale_coord) # switch axes in order to have proper format for next function, decompose_rel_hmg.py H_opt_rel_coord = H_opt_rel_coord.swapaxes(0, 2) H_opt_rel_coord = H_opt_rel_coord.swapaxes(0, 1) print("平均精度为:", np.sum(np.array(distance_error_list)) / 17) sio.savemat( opt_param_save_loc, dict([('H_rel', H_opt_rel_coord), ('cor_opt', xy_cor_list_opt), ('diff_score', diff_score_list)]))
def read_housing_csv_2(self, file_name, x_mapping_state, target_name=None): skip_header = 1 if self.config.NN_MULTI_ENCODE_TEXT_VARS or self.config.NN_APPLY_DATA_SCIENCE: skip_header = 0 data = np.genfromtxt(file_name, delimiter=',', dtype='unicode', skip_header=skip_header) if (self.config.NN_DEBUG_SHAPES): print(data.shape) if (target_name is None): skip_cols = 1 else: skip_cols = 2 # Identify Area columns area_cols = [i for i, item in enumerate(data[0, :]) if "Area" in item] # multi-encode if self.config.NN_MULTI_ENCODE_TEXT_VARS: X_data, self.neighborhood_vals = multi_encode_text_variables( "Neighborhood", data, self.neighborhood_vals) X_data = np.delete(X_data, 0, axis=0) # Remove header now data = X_data if self.config.NN_APPLY_DATA_SCIENCE: # Apply some data science data = self.filter_training_data(data, target_name=target_name) X_data = self.augment_training_data(data, target_name=target_name) X_data = np.delete(X_data, 0, axis=0) # Remove header now data = X_data # Get (samplesize x features per sample) X = np.empty( (data.shape[0], data.shape[1] - skip_cols)) # Dont need Id and Price columns for col in range(data.shape[1] - skip_cols): map_id = 1.0 # reset every feature if (target_name is not None): mapping_state = {} else: mapping_state = x_mapping_state[col] if col in area_cols: # Direct mapping for row in range(data.shape[0]): try: X[row][col] = data[row][col + 1].astype(float) except: if (data[row][col + 1] in mapping_state): X[row][col] = mapping_state[data[row][col + 1]] else: mapping_state[data[row][col + 1]] = map_id X[row][col] = map_id map_id = map_id + 1.0 else: for row in range(data.shape[0]): if (data[row][col + 1] in mapping_state): X[row][col] = mapping_state[data[row][col + 1]] else: mapping_state[data[row][col + 1]] = map_id X[row][col] = map_id map_id = map_id + 1.0 x_mapping_state.append(mapping_state) # Get groundtruths Y = np.empty((data.shape[0], 1)) if (target_name is not None): prev = 0.0 for row in range(data.shape[0]): col = data.shape[1] - 1 try: Y[row][0] = data[row][col].astype(float) except: raise Exception("Ground truth should be float") # Ensure GT was sorted before # assert (prev <= Y[row][0]) prev = Y[row][0] if self.config.NN_LOG_TARGET is True: Y[row][0] = np.log(Y[row][0]) # Normalize Y_normalize_state = X_normalize_state = None if (self.config.NN_NORMALIZE): if (target_name is not None): Y, Y_normalize_state = self.utils.normalize0(Y, axis=0) X, X_normalize_state = self.utils.normalize0(X, axis=0) if (self.config.NN_DEBUG_SHAPES): print(X.shape, Y.shape, X, X[0][0].dtype) return X, X_normalize_state, x_mapping_state, Y, Y_normalize_state