-
Notifications
You must be signed in to change notification settings - Fork 5
/
mainEntrance_for_realworld.m
executable file
·126 lines (96 loc) · 4.71 KB
/
mainEntrance_for_realworld.m
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
%%
% -------------------------------------------------------------------------------------------------------
% This code is for ICRA 2018 paper " Multi-view Vihcle Localization Based on Keypoint Detection ConvNet "
% we assume that you have got heatmaps from CNN, and this code can estimate car pose from multiple
% camera views (all calibrated).
%
% Author : Wenhao Ding
% Data : 8/8/2017
% Copyright 2017 | All right reserved.
% -------------------------------------------------------------------------------------------------------
%%
clear all; clc; close all;
warning('off');
addpath(genpath('utils'));
addpath(genpath('other_tools/paper_figures'));
% path for annotations
datapath = './realworld/';
% path for network output
predpath = './realworld/';
% path where the results are stored
savepath = 'result/';
% define some parameters
count = 1;
% this number can be arbitried, because any pair of calibration is ok
cam_pair = 2;
% this list is for image to be shown
camera_list = [2,5];
% load parameters
all_file_name = importdata(strcat(datapath, 'valid_images.txt'));
all_scale = h5read(strcat(datapath, 'valid.h5'),'/scale');
all_center = h5read(strcat(datapath, 'valid.h5'),'/center');
load(strcat(datapath, 'camera.mat'));
cad = load(strcat(datapath, 'car.mat'));
cad = cad.('car');
cad = cad(2);
dict = getPascalTemplate(cad);
model = centralize(dict.mu)';
% set initial model scale
model = model.*85;
%% --- input ---
for camera_ID = 1 : camera.camera_number
imgname{camera_ID} = all_file_name(camera_list(camera_ID));
center{camera_ID} = all_center(:,camera_list(camera_ID));
scale{camera_ID} = all_scale(camera_list(camera_ID));
% read heatmaps and detect maximum responses
heatmap{camera_ID} = h5read(sprintf('%s/valid_%d.h5', predpath, camera_list(camera_ID)), '/heatmaps');
% rotate the heatmap
heatmap{camera_ID} = permute(heatmap{camera_ID}(:,:,46:57),[2,1,3]);
% heatpoint_2D means the 2D coordinate of those keypoints
[heatpoint_2D{camera_ID}, D{camera_ID}] = findWmax(heatmap{camera_ID});
% transform form heatmap coordinate to <full image> coordinate
W_im{camera_ID} = transformHG(heatpoint_2D{camera_ID}, center{camera_ID}, scale{camera_ID}, size(heatmap{camera_ID}(:,:,1)), true);
W_hg{camera_ID} = camera.cam{camera_ID}.K\[W_im{camera_ID};ones(1,size(W_im{camera_ID},2))];
end
%% --- start to estimate pose ---
% - pose optimization - perspective projection
% - use common points to get a initial pose, and [keypoint_common] saves the common points
[keypoint_common, initialPose] = getInitialPose(W_hg, D, model, camera);
fprintf('Finishing initializing pose... \n');
% - use cameras' views to estimated pose
[fval, estimated_pose] = PoseEstimaedFromMultiview(heatpoint_2D, W_hg, initialPose, D, model, camera, center, scale);
fprintf('Finishing estimateing pose... \n');
%% --- visualization ---
% draw 3D camera and car pose relationship
% nplot = 3;
% figure('position', [300,300,nplot*400,400]);
% subplot('position', [0,0,1/nplot,1]);
% figure
% drawCameraAndCar(initialPose.R1, initialPose.T1, estimated_pose.S, camera, 1);
% showCameraForRealworld(stereoParams.CameraParameters1, cam_pair, 1); hold on
% showCameraForRealworld(stereoParams.CameraParameters2, cam_pair, 2);
% title('Initial Pose');axis equal
% subplot('position',[1/nplot,0,1/nplot,1]);
figure
drawCameraAndCar(estimated_pose.R1, estimated_pose.T1, estimated_pose.S, camera, 1);
showCameraForRealworld(stereoParams.CameraParameters1, cam_pair, 1); hold on
showCameraForRealworld(stereoParams.CameraParameters2, cam_pair, 2);
title('Left Estimated Pose'); axis equal
% subplot('position',[2/nplot,0,1/nplot,1]);
figure
drawCameraAndCar(estimated_pose.R2, estimated_pose.T2, estimated_pose.S, camera, 2);
showCameraForRealworld(stereoParams.CameraParameters1, cam_pair, 1); hold on
showCameraForRealworld(stereoParams.CameraParameters2, cam_pair, 2);
title('Right Estimated Pose'); axis equal
% - read imges
for camera_ID = 1 : camera.camera_number
img_to_be_shown{camera_ID} = imread(sprintf('%s/%s',datapath, char(imgname{camera_ID})));
end
% draw heatmap and projection
[L, R] = visualize_all(heatpoint_2D, fval, img_to_be_shown, estimated_pose, heatmap, center, scale, camera, estimated_pose.S);
drawBigFigure(heatpoint_2D, img_to_be_shown, estimated_pose, heatmap, center, scale, camera, estimated_pose.S);
% showBetterThanMono(img_to_be_shown, estimated_pose, heatmap, center, scale, camera, estimated_pose.S);
%% --- output ---
%savefile = sprintf('%s/valid_%d.mat', savepath, count);
%count = count+1;
%save(savefile, 'estimated_pose');