-
Notifications
You must be signed in to change notification settings - Fork 3
/
RmsErrorPointsToPlanes.m
33 lines (28 loc) · 961 Bytes
/
RmsErrorPointsToPlanes.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
function RmsError = RmsErrorPointsToPlanes(var, Lpts, LsrSegLen, Nc, Dc)
% INPUTS:
% Lpts: 3xN vector containing all the laser board points.
% LsrSegLen: Mx1 vector containing the scan numbers of the laser points.
% Nc: M*3 vector containing the normals of chessboard in camera frame.
% Dc: Mx1 distance vector of camera origin to chessboard
% OUTPUTS:
% RmsError: root mean squares of errors.
if(size(Lpts, 2) == 3)
Lpts = Lpts';
end
R = rodrigues(var(1:3));
t = var(4:6);
%pre-allocate memory
allPoints = size(Lpts, 2);
NC = zeros(3, allPoints);
DC = zeros(1, allPoints);
index = 1;
endPos = 0;
for i=1:length(LsrSegLen)
endPos = endPos + LsrSegLen(i);
NC(1:3, index:endPos) = repmat(Nc(i, :)', 1, LsrSegLen(i));
DC(1, index:endPos) = repmat(Dc(i,:), 1, LsrSegLen(i));
index = endPos+1;
end
% 3*N 3x3*3xN + 3*1
errorvec = dot(NC, (R*Lpts + t)) - DC;
RmsError = sqrt( sum(errorvec.^2)/length(errorvec) );