52 lines
1.0 KiB
Matlab
52 lines
1.0 KiB
Matlab
function [ measurements ] = project_landmarks( pose, landmarks, K )
|
|
%UNTITLED3 Summary of this function goes here
|
|
% Detailed explanation goes here
|
|
|
|
import gtsam.*;
|
|
|
|
camera = PinholeCameraCal3_S2(pose,K);
|
|
measurements = Values;
|
|
|
|
for i=1:size(landmarks)-1
|
|
z = camera.project(landmarks.atPoint3(symbol('l',i)));
|
|
|
|
% check bounding box
|
|
if z(1) < 0 || z(1) > 1280
|
|
continue
|
|
elseif z(2) < 0 || z(2) > 960
|
|
continue
|
|
end
|
|
|
|
measurements.insert(symbol('z',i),z);
|
|
end
|
|
|
|
% persistent h;
|
|
%
|
|
% if isempty(h)
|
|
% h = figure();
|
|
% else
|
|
% figure(h);
|
|
% end
|
|
% clf;
|
|
|
|
if measurements.size == 0
|
|
return
|
|
end
|
|
|
|
cla;
|
|
plot2DPoints(measurements,'*g');
|
|
|
|
text(1120, 1000, sprintf('# = %d', measurements.size));
|
|
|
|
axis equal;
|
|
axis([0 1280 0 960]);
|
|
|
|
set(gca, 'YDir', 'reverse');
|
|
xlabel('u (pixels)');
|
|
ylabel('v (pixels)');
|
|
set(gca, 'XAxisLocation', 'top');
|
|
|
|
|
|
end
|
|
|