36 lines
928 B
Matlab
36 lines
928 B
Matlab
function plotProjectedCylinderSamples(visiblePoints3, cameraPose, figID)
|
|
% plot the visible projected points on the cylinders
|
|
% author: Zhaoyang Lv
|
|
|
|
import gtsam.*
|
|
|
|
figure(figID);
|
|
|
|
holdstate = ishold;
|
|
hold on
|
|
|
|
%plotCamera(cameraPose, 5);
|
|
|
|
pointsNum = size(visiblePoints3, 1)
|
|
|
|
for i=1:pointsNum
|
|
ray = visiblePoints3{i}.between(cameraPose.translation()).vector();
|
|
dist = norm(ray);
|
|
|
|
p = plot3(visiblePoints3{i}.x, visiblePoints3{i}.y, visiblePoints3{i}.z, ...
|
|
'o', 'MarkerFaceColor', 'Green');
|
|
|
|
for t=0:0.1:dist
|
|
marchingRay = ray * t;
|
|
p.XData = visiblePoints3{i}.x + marchingRay(1);
|
|
p.YData = visiblePoints3{i}.y + marchingRay(2);
|
|
p.ZData = visiblePoints3{i}.z + marchingRay(3);
|
|
drawnow update
|
|
end
|
|
|
|
end
|
|
|
|
if ~holdstate
|
|
hold off
|
|
end
|
|
end |