-
Notifications
You must be signed in to change notification settings - Fork 2
/
computePoseFromLiDARToMocapMarkers.m
46 lines (37 loc) · 1.62 KB
/
computePoseFromLiDARToMocapMarkers.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
function out_t = ...
computePoseFromLiDARToMocapMarkers(L_X, M_Y, template, corner_order)
% X: Target vertices estiamted from the lidar frame
% Y: Mocap makers measured from the mocap frame
% Assume X and Y are the same but measured from different sensors,
% Here, the sensors are mocap and lidar
% Transform data from 3xn to nx3
L_X = L_X'; % n x 3
M_Y = M_Y'; % n x 3
% Construct template at the lidar origin
template = template(:, corner_order)';
% Estimate H from Y to X, which results in Y measured in LiDAR frame
[cost_ML, L_Y, ~] = procrustes(L_X, M_Y, 'scaling', 0, 'reflection', 0);
% [cost_ML, ~, transform] = procrustes(M_Y, L_X, 'scaling', 0, 'reflection', 0);
% H_XY = eye(4);
% H_XY(1:3, 1:3) = transform.T';
% H_XY(1:3, 4) = transform.c(1, :)';
% L_Y = H_XY \ convertToHomogeneousCoord(M_Y');
% L_Y = L_Y(1:3, :)';
cost_ML
% Esimate the H from the template at the lidar origin to the markers at
% LiDAR frame (L_marker)
[cost_LT, ~, transform] = procrustes(L_Y, template, 'scaling', 0, 'reflection', 0);
cost_LT
% L_template_to_target and L_Y should be close to each other
H_ML = eye(4);
H_ML(1:3, 1:3) = transform.T';
H_ML(1:3, 4) = transform.c(1, :)';
L_template_to_target = H_ML * convertToHomogeneousCoord(template');
out_t.cost_ML = cost_ML;
out_t.cost_LT = cost_LT;
out_t.L_markers = L_Y;
out_t.L_template_to_target = L_template_to_target;
out_t.L_H_LT = H_ML;
out_t.translation = H_ML(1:3, 4)';
out_t.rpy = rad2deg(rotm2eul(H_ML(1:3, 1:3), "XYZ"));
end