39 lines
1.1 KiB
Matlab
39 lines
1.1 KiB
Matlab
function [graph,initial] = load3D(filename,model,N)
|
|
% load3D: read TORO 3D pose graph
|
|
% cannot read noise model from file yet, uses specified model
|
|
fid = fopen(filename);
|
|
if fid < 0
|
|
error(['load2D: Cannot open file ' filename]);
|
|
end
|
|
|
|
% scan all lines into a cell array
|
|
columns=textscan(fid,'%s','delimiter','\n');
|
|
fclose(fid);
|
|
lines=columns{1};
|
|
|
|
% loop over lines and add vertices
|
|
graph = pose3SLAMGraph;
|
|
initial = pose3SLAMValues;
|
|
n=size(lines,1);
|
|
if nargin<3, N=n;end
|
|
|
|
for i=1:n
|
|
line_i=lines{i};
|
|
if strcmp('VERTEX3',line_i(1:7))
|
|
v = textscan(line_i,'%s %d %f %f %f %f %f %f',1);
|
|
if v{2}<N
|
|
t = gtsamPoint3(v{3}, v{4}, v{5});
|
|
R = gtsamRot3_ypr(v{6}, v{7}, v{8});
|
|
initial.insertPose(v{2}, gtsamPose3(R,t));
|
|
end
|
|
elseif strcmp('EDGE3',line_i(1:5))
|
|
e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
|
|
if e{2}<N & e{3}<N
|
|
t = gtsamPoint3(e{4}, e{5}, e{6});
|
|
R = gtsamRot3_ypr(e{7}, e{8}, e{9});
|
|
graph.addConstraint(e{2}, e{3}, gtsamPose3(R,t), model);
|
|
end
|
|
end
|
|
end
|
|
|