i have positioned the zed2 2D according to the matlab instructions on the page. but i don’t know how to build a map and locate it in it using matlab. Hope that someone can help me, thank you very much!
Hi,
I guess you want to save an area map and then reload it.
Here is a sample to do so:
clc;
disp('========= ZED SDK PLUGIN =========');
disp('-- Get ZED Camera Position --');
close all;
clear mex; clear functions; clear all;
% initial parameter structure, the same as sl::InitParameters
% values as enum number, defines in : sl/defines.hpp
% or from https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html
InitParameters.svo_input_filename = 'D:/SVO/castle/full.svo'; % Enable SVO playback
result = mexZED('open', InitParameters);
if(strcmp(result,'SUCCESS'))
%enable Tracking
PositionalTrackingParameters.enable_spatial_memory = 1;
mexZED('enablePositionalTracking', PositionalTrackingParameters);
frame = 0;
reloc_frame = 200;
while (frame < (reloc_frame + 20))
% grab the current image and compute the positional tracking
result = mexZED('grab');
if(strcmp(result,'SUCCESS'))
% retrieve camera Path
[position, state] = mexZED('getPosition');
frame = frame+1;
if(frame >= reloc_frame)
% display Camera Position around the requested reloc
position
end
end
end
% disable Tracking and export area file
mexZED('disablePositionalTracking' , 'MyAreaMap.area');
% get back to a previous frame
frame = reloc_frame;
mexZED('setSVOPosition', frame);
mexZED('grab');
% re enable Tracking and load the saved Area Map
PositionalTrackingParameters.enable_spatial_memory = 1;
PositionalTrackingParameters.area_file_path = 'MyAreaMap.area';
mexZED('enablePositionalTracking', PositionalTrackingParameters);
while (frame < (reloc_frame + 20))
% grab the current image and compute the positional tracking
result = mexZED('grab');
if(strcmp(result,'SUCCESS'))
% retrieve camera Path
[position, state] = mexZED('getPosition');
% check the state of the Tracking to know when it is relocated
state
if(strcmp(state,'OK'))
% print the position, should be similar to the saved one
position
end
frame = frame+1;
end
end
end
% Make sure to call this function to free the memory before use this again
mexZED('close')
disp('========= END =========');
clear mex;
I have also updated the mex file to export the Positional Tracking state to check if the camera is well relocated in the given area file.
Thanks you.
And so, this is my purpose:
- Tracking position (stream) and then save an area map, after I press ESC. (
- Next, I turn on again tracking in this area and get its position.
But I can’t get my area file, this is an error.
Hope so can help me.
clc;
disp(’========= ZED SDK PLUGIN =========’);
disp(’-- Get ZED Camera Position --’);
close all;
clear mex; clear functions; clear all;
InitParameters.camera_resolution = 2; %HD720
InitParameters.camera_fps = 60;
InitParameters.coordinate_units = 2; %METER
InitParameters.depth_mode = 1; %PERFORMANCE
InitParameters.coordinate_system = 3; %COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP
%InitParameters.svo_input_filename = ‘MySVO.svo’; % Enable SVO playback
result = mexZED(‘open’, InitParameters);
if(strcmp(result,‘SUCCESS’))
%enable Tracking
PositionalTrackingParameters.enable_spatial_memory = 1;
mexZED(‘enablePositionalTracking’, PositionalTrackingParameters);
% for tracking informations storage
PositionArray1 = [];
PositionArray2 = [];
% Create Figure and wait for keyboard interruption to quit
f = figure('name','ZED SDK : Positional Tracking','NumberTitle','off','keypressfcn',@(obj,evt) 0);
%create 2 sub figure
ha1 = axes('Position',[0.05,0.7,0.9,0.25]);
ha2 = axes('Position',[0.05,0.05,0.9,0.6]);
xlabel('Tx (M)');
ylabel('Tz (M)');
zlabel('Ty (M)');
xlim(ha2, [-2 2]);
ylim(ha2, [-2 2]);
axis equal, grid on;
hold on;
% init 3d display
h = plot3(0,0,0, 'r');
key = 1;
% loop over frames, till Esc is pressed
while (key ~= 27)
% grab the current image and compute the positional tracking
result = mexZED('grab');
if(strcmp(result,'SUCCESS'))
% retrieve letf image
image_left = mexZED('retrieveImage', 0); %left
%displays it
axes(ha1);
imshow(image_left);
% retrieve camera Path
[position, state] = mexZED('getPosition');
%stack positions
PositionArray1 = [PositionArray1; position(1,4) position(2,4) position(3,4)];
axes(ha2);
set(h,'XData',PositionArray1(:,1))
set(h,'YData',PositionArray1(:,2))
set(h,'ZData',PositionArray1(:,3))
drawnow; %this checks for interrupts
key = uint8(get(f,'CurrentCharacter'));
length(key);
if(~length(key))
key=0;
end
end
end
% disable Tracking and export area file
mexZED('disablePositionalTracking' ,'Myymap.area');
disp('End tracking 1st');
disp('Relocalization');
% re enable Tracking and load the saved Area Map
% Loading an area file will start a search phase, during which the camera will try to position itself in the previously learned area.
PositionalTrackingParameters.enable_spatial_memory = 1;
PositionalTrackingParameters.enable_pose_smoothing = 1;
PositionalTrackingParameters.area_file_path = 'Myymap.area';
mexZED('enablePositionalTracking', PositionalTrackingParameters);
pause(3);
key = 1;
flag = 1;
while (key ~= 27)
result = mexZED('grab');
% grab the current image and compute the positional tracking
if(strcmp(result,'SUCCESS'))
image_left = mexZED('retrieveImage', 0); %left
%displays it
axes(ha1);
imshow(image_left);
% retrieve camera Path
[position, state] = mexZED('getPosition');
% check the state of the Tracking to know when it is relocated
state
if(strcmp(state,'OK'))
% print the position, should be similar to the saved one
PositionArray2 = [PositionArray2; position(1,4) position(2,4) position(3,4)]
if (flag == 1)
xo = position(1,4);
yo = position(2,4);
zo = position(3,4);
h = plot3(xo,yo,zo,'g');
flag = 0
end
axes(ha2);
set(h,'XData',PositionArray2(:,1))
set(h,'YData',PositionArray2(:,2))
set(h,'ZData',PositionArray2(:,3))
drawnow; %this checks for interrupts
end
end
key = uint8(get(f,'CurrentCharacter'));
if(~length(key))
key=0;
end
end
end
% Make sure to call this function to free the memory before use this again
mexZED(‘close’)
disp(’========= END =========’);
clear mex;
In this code, I can’t find “Myymap.area” file.
:(((