-
Notifications
You must be signed in to change notification settings - Fork 7
/
costFunction_L2I_12.m
23 lines (23 loc) · 1.21 KB
/
costFunction_L2I_12.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
function loss = costFunction_L2I_12(pose_1, pose_2, x0)
mu = [1, 1, 1e6, 1e6]; % weight
R = [x0(1, 4 : 6); x0(1, 7 : 9); x0(1, 10 : 12)];
t = x0(1, 1 : 3)';
loss = 0;
[n, ~] = size(pose_1);
for i = 2 : n
[R_1, t_1] = calcRelativePose_quat(pose_1(i - 1, :), pose_1(i, :));
[R_2, t_2] = calcRelativePose_quat(pose_2(i - 1, :), pose_2(i, :));
% loss = loss + mu(1) * norm(R_1 * R - R * R_2, 2) + ...
% mu(2) * norm(R_1 * t + t_1 - R * t_2 - t, 2); % Worse
% loss = loss + mu(1) * norm(R_1 * R - R * R_2, 2) + ...
% mu(2) * norm(R_1 * t + t_1 - R * t_2 - t, 2) + ...
% mu(2) * norm(x0(1, 1 : 3) - [0.35, 0, -0.13], 2); % Consider Measurement
loss = loss + mu(1) * (norm(R_1 * R - R * R_2, 2))^2 + ...
mu(2) * (norm(R_1 * t + t_1 - R * t_2 - t, 2))^2; % Work
% loss = loss + mu(1) * sum(sum((R_1 * R - R * R_2).^2)) + ...
% mu(2) * sum(sum((R_1 * t + t_1 - R * t_2 - t).^2)); % Test
end
% loss = loss + mu(3) * norm(R * R' - eye(3), 2) + mu(4) * norm(R * R' - eye(3), 2); % Worse
loss = loss + mu(3) * norm(R * R' - eye(3), 2)^2 + mu(4) * norm(R * R' - eye(3), 2)^2; % Work
% loss = loss + mu(3) * sum(sum((R * R' - eye(3)).^2)) + mu(4) * sum(sum((R * R' - eye(3)).^2)); % Test
end