-
Notifications
You must be signed in to change notification settings - Fork 2
/
fv.asv
134 lines (107 loc) · 3.43 KB
/
fv.asv
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
%-----------------------------------------------------------------------
% 1-point RANSAC EKF SLAM from a monocular sequence
%-----------------------------------------------------------------------
% Copyright (C) 2010 Javier Civera and J. M. M. Montiel
% Universidad de Zaragoza, Zaragoza, Spain.
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation. Read http://www.gnu.org/copyleft/gpl.html for details
% If you use this code for academic work, please reference:
% Javier Civera, Oscar G. Grasa, Andrew J. Davison, J. M. M. Montiel,
% 1-Point RANSAC for EKF Filtering: Application to Real-Time Structure from Motion and Visual Odometry,
% to appear in Journal of Field Robotics, October 2010.
%-----------------------------------------------------------------------
% Authors: Javier Civera -- jcivera@unizar.es
% J. M. M. Montiel -- josemari@unizar.es
% Robotics, Perception and Real Time Group
% Arag�n Institute of Engineering Research (I3A)
% Universidad de Zaragoza, 50018, Zaragoza, Spain
% Date : May 2010
%-----------------------------------------------------------------------
function [X_k_km1,varargout]=fv(X_k_k,delta_t, type, std_a, std_alpha)
rW =X_k_k(1:3,1);
qWR=X_k_k(4:7,1);
vW =X_k_k(8:10,1);
wW =X_k_k(11:13,1);
%%% TAMADD
global step_global
global myCONFIG
%%% From RANSAC
%% DEBUG TEST DR YE's VODOMETRY
% [dX_gt,dq_calc]=Calculate_V_Omega_RANSAC_my_version(step_global-1,step_global);
if step_global-1<=1
dX_gt=[0;0;0];
dq_calc=[1;0;0;0];
R = eye(3);
State_RANSAC =1;
else
[dX_gt,dq_calc,R,State_RANSAC]=Calculate_V_Omega_RANSAC_dr_ye(step_global-1,step_global-1);
end
vW = q2R(qWR)*dX_gt/delta_t; %%% FIXIT test, transforamtion from GT and rotation from RANSAC
wW = q2v(dq_calc)/delta_t;
if nargout==5
varargout{1} = vW;
varargout{2} = wW;
varargout{3} = dq_calc;
varargout{4} = dX_gt;
end
%%%
if strcmp(type,'constant_orientation')
wW = [0 0 0]';
X_k_km1=[rW+vW*delta_t;
qWR;
vW;
wW];
end
if strcmp(type,'constant_position')
vW = [0 0 0]';
X_k_km1=[rW;
reshape(qprod(qWR,v2q(wW*delta_t)),4,1);
vW;
wW];
end
if strcmp(type,'constant_position_and_orientation')
vW = [0 0 0]';
wW = [0 0 0]';
X_k_km1=[rW;
qWR;
vW;
wW];
end
if strcmp(type,'constant_position_and_orientation_location_noise')
vW = [0 0 0]';
wW = [0 0 0]';
X_k_km1=[rW;
qWR;
vW;
wW];
end
%%% ORIGINAL CODE
% % % % % if strcmp(type,'constant_velocity')
% % % % % X_k_km1=[rW+vW*delta_t;
% % % % % %% DEBUG
% % % % % R2q(q2R(dq_calc)*q2R(qWR))
% % % % %
% % % % % % reshape(qprod(qWR,v2q(wW*delta_t)),4,1);
% % % % % vW;
% % % % % wW];
% % % % % end
if strcmp(type,'constant_velocity')
H = [q2R(qWR),rW;0 0 0 1];
H = H*Pose2H([dX_gt;q2e(dq_calc)]);
% X_k_km1=[rW+vW*delta_t;
%% DEBUG
% R2q(q2R(dq_calc)*q2R(qWR))
% reshape(qprod(qWR,v2q(wW*delta_t)),4,1);
% vW;
% wW];
end
X_k_km1 = [H(1:3,4);R2q(H(1:3,1:3));vW;wW];
%%% TAMADD
% if strcmp(type,'constant_velocity')
% X_k_km1=[rW+vW*delta_t;
% reshape(qprod(qWR,q_calc),4,1);
% vW;
% wW];
% end
%%%