%% -----------------------------------------------------------------------------
%% navigates a car in a virtual world by capturing the virtual camera view of the car while navigating
%% -----------------------------------------------------------------------------
%% VR world:
%% distances are in meters, angles in radians
%% coordinate system (x,y,z) where x goes to the left, y upwards, and z towards the viewer
function ofnav
global glob
glob.active = 0; % robot activation switch
glob.images = []; % image data
glob.frame = 0; % current frame number
glob.planeHeight = 400; % image plane height
glob.planeWidth = 400; % image plane width
glob.speed = 0.05; % speed of the car
glob.angle = -pi; % start angle
glob.steer_angle = 0; % steering angle
glob.vpos = [-8 0 0]; % start position of the car
glob.VF = []; % deltaf values
glob.TTC = []; % time-to-contact values
glob.rotframes = 0; % remaining rotation frames
%% Open and View the World
if (~isfield(glob, 'world'))
%%world = vrworld('mountain.wrl');
glob.world = vrworld('obstacles.wrl');
open(glob.world);
else
reload(glob.world);
end
% calculate focalDistance
glob.viewpoint = vrnode(glob.world, 'VPfollow');
fieldOfView = glob.viewpoint.fieldOfView
focalDistance = glob.planeHeight * 0.5 / tan(fieldOfView * 0.5)
% It may take some time before the viewer opens, so please be patient.
if (~isfield(glob, 'fig'))
glob.fig = view(glob.world, '-internal');
set(glob.fig, 'Position', [10, 150, glob.planeWidth, glob.planeHeight])
set(glob.fig,'DeleteFcn','vrcloseaction');
end
set(glob.fig,'WindowCommand','raise');
% Before capture, it is recommended to set the navigation panel
% to one of the modes in which it doesn't overlap the virtual scene
set(glob.fig,'NavPanel','none');
set(glob.fig,'Toolbar','off');
set(glob.fig,'StatusBar','off');
set(glob.fig,'NavMode','none');
% disable lighting
%%set(glob.fig, 'Lighting', 'off');
% choose the driver's view (camera)
set(glob.fig, 'Viewpoint', 'View 2 - Driver');
glob.viewpoint.position = glob.vpos;
glob.viewpoint.orientation = [0, 1, 0, glob.angle - 1/2 * pi];
vrdrawnow;
% init user interface
glob.gui=figure('units','pixels','position',[450 200 800 450]);
set(glob.gui, 'BackingStore', 'off');
%%set(glob.gui, 'DoubleBuffer', 'off');
set(glob.gui, 'toolbar', 'none');
set(glob.gui, 'MenuBar', 'none');
set(glob.gui, 'name', 'GUI');
%%set(glob.gui, 'WindowStyle', 'modal');
set(glob.gui,'CloseRequestFcn',@closeGUI);
uicontrol('style','pushbutton','string','Start','position',[20 10 100 30], ...
'BackgroundColor', [0.5 1 0.5], 'callback',@startstop)
uicontrol('style','text','string','Frame','position',[20 50 100 20])
uicontrol('style','text','string','Left flow','position',[20 70 100 20])
uicontrol('style','text','string','Right flow','position',[20 90 100 20])
glob.text_frame = uicontrol('style','text','position',[120 50 100 20])
glob.text_leftflow = uicontrol('style','text','position',[120 70 100 20])
glob.text_rightflow = uicontrol('style','text','position',[120 90 100 20])
glob.axis1 = axes('units','pixels','position',[50 150 200 200],'xlim',[-1 1],'ylim',[-1 1])
uicontrol('style','text','string','deltaf','position',[50 370 100 20])
% flow field
glob.axis2 = axes('units','pixels','position',[280 150 200 200],'xlim',[0 100],'ylim',[0 100])
glob.checkbox_updateflow = uicontrol('style','checkbox','string','Update flow field','position',[300 100 200 20])
uicontrol('style','text','string','flow field','position',[280 370 100 20])
% TTC graph
glob.axis3 = axes('units','pixels','position',[530 150 200 200],'xlim',[-1 1],'ylim',[-1 1])
uicontrol('style','text','string','time-to-contact','position',[530 370 100 20])
%% ---------------------------------------------------------------------------
%% loop that captures the camera view and controls the car
%% ---------------------------------------------------------------------------
function startstop(obj, cancel)
global glob
glob.active = ~glob.active;
if (~glob.active)
set(obj,'String', 'Start', 'BackgroundColor', [0.5 1 0.5])
return
end
set(obj,'String', 'Stop', 'BackgroundColor', [1 0.5 0.5])
ImageBlur = 0.04 * ones(5,5);
vmax = 0;
while glob.active
x = cos(glob.angle) * glob.speed;
z = sin(glob.angle) * glob.speed;
vdir = [x 0 -z];
glob.vpos = glob.vpos + vdir;
% vector (x,0,z) to move viewpoint to scene position
glob.viewpoint.position = glob.vpos;
% vector (0,y,0,alpha) to rotate viewpoint around y-axis
glob.viewpoint.orientation = [0, 1, 0, glob.angle - 1/2 * pi];
set(glob.world,'Time',glob.frame);
vrdrawnow;
% capture the driver's camera view
img_capture = vrfigcapture(glob.fig);
img = rgb2gray(img_capture);
tmp = imresize(img,0.25,'nearest');
%%tmp = flipdim(img, 1);
%%tmp = imfilter(tmp, ImageBlur);
glob.images(:,:,1) = tmp;
if ((size(glob.images,3) >= 2) && (glob.rotframes == 0))
[Vx, Vy] = OpticalFlowMatlab(glob.images, 1, 4); %% images, alpha, iterations
% calculate magnitude of optical flow for each half of the camera view
Vmag = sqrt(Vx.^2 + Vy.^2);
Vmag = flipdim(Vmag, 1);
%imagesc(Vmag);
VL = Vmag(:, 1:size(Vmag,2)/2);
VR = Vmag(:, size(Vmag,2)/2:size(Vmag,2));
vl = sum(VL(:));
vr = sum(VR(:));
ttc = 1 / sum(Vmag(:)) * 100.0;
glob.TTC = [glob.TTC ttc];
if (size(glob.TTC,2) > 20)
glob.TTC = glob.TTC(2:end);
end
% turn view away from side of greater flow
% set steering angle (in radiant): steer_angle < 0 turns to left, steer_angle > 0 turn to right
deltaf = (vl - vr) / (vl + vr);
glob.VF = [glob.VF deltaf];
if (size(glob.VF,2) > 20)
glob.VF = glob.VF(2:end);
end
if (ttc < 0.01)
if (glob.rotframes == 0)
glob.steer_angle = -1/32 * pi * sign(deltaf);
glob.rotframes = 13;
glob.speed = 0;
end
end
if (get(glob.checkbox_updateflow, 'value'))
xgrid = 1:5:size(Vx,2);
ygrid = 1:5:size(Vx,1);
[xi,yi]=meshgrid(xgrid, ygrid);
Vxi = interp2(Vx, xi, yi);
Vyi = interp2(Vy, xi, yi);
axes(glob.axis2)
imshow(tmp)
hold on;
vmax = max(vmax, max(Vyi(:)));
Vx(1,1) = vmax;
Vy(1,1) = vmax;
quiver(xgrid, ygrid, Vxi, -Vyi, 2, 'y');
axis([0 100 0 100]);
%% draw focus of expansion (FOE)
[x, y] = foe(Vx, Vy);
plot(x, y, 'go', 'LineWidth', 2);
hold off;
end
% update user interface
set(glob.text_leftflow,'String', sprintf('%.0f', vl))
set(glob.text_rightflow,'String', sprintf('%.0f', vr))
set(glob.text_frame,'String', sprintf('%d', glob.frame))
%%set(0,'CurrentFigure',glob.axis2)
% plot deltaf values
axes(glob.axis1)
plot(glob.VF)
set(glob.axis1,'YLim',[-1 1])
% plot TTC values
axes(glob.axis3)
plot(glob.TTC)
set(glob.axis3,'YLim',[0 0.03])
drawnow
end
glob.images(:,:,2) = glob.images(:,:,1);
if (glob.rotframes > 0)
glob.rotframes = glob.rotframes - 1;
end
if (glob.rotframes == 1)
glob.steer_angle = 0;
end
if (glob.rotframes == 0)
glob.speed = 0.05;
end
glob.angle = glob.angle + glob.steer_angle;
glob.frame = glob.frame + 1;
end
%% ---------------------------------------------------------------------------
%% called when closing the GUI
%% ---------------------------------------------------------------------------
function closeGUI(src,evnt)
%global glob
%glob.active = 0;
%%pause(0.2)
%%disp('bye');
%close(glob.world)
%delete(glob.world)
delete(gcf)
