12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change lightings and add flying through sequence

release/4.3a0
lvzhaoyang 2015-01-22 00:25:06 -05:00
parent 2627f9a9cd
commit fa023aac1a
2 changed files with 37 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matlab/+gtsam/plotCamera.m
View File

@ -4,15 +4,15 @@ function plotCamera(pose, axisLength)
xAxis = C+R(:,1)*axisLength; xAxis = C+R(:,1)*axisLength;
L = [C xAxis]'; L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r'); h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
yAxis = C+R(:,2)*axisLength; yAxis = C+R(:,2)*axisLength;
L = [C yAxis]'; L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g'); h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
zAxis = C+R(:,3)*axisLength; zAxis = C+R(:,3)*axisLength;
L = [C zAxis]'; L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b'); h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
axis equal axis equal
end end

59
matlab/+gtsam/plotFlyingResults.m
View File

@ -19,7 +19,10 @@ end
figID = 1; figID = 1;
figure(figID); figure(figID);
view([30, 0]); view([30, 30]);
hlight = camlight('headlight');
lighting gouraud
sampleDensity = 120; sampleDensity = 120;
cylinderNum = length(cylinders); cylinderNum = length(cylinders);
@ -31,7 +34,8 @@ for i = 1:cylinderNum
Z = Z * cylinders{i}.height; Z = Z * cylinders{i}.height;
h_cylinder = surf(X,Y,Z); h_cylinder = surf(X,Y,Z);
set(h_cylinder, 'FaceColor', [0 0 0.5], 'FaceAlpha', 0.2, 'EdgeColor', [0 0 1]); set(h_cylinder, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
h_cylinder.AmbientStrength = 1;
hold on hold on
end end
@ -53,23 +57,25 @@ for i = 1:posesSize
if exist('h_cov', 'var') if exist('h_cov', 'var')
delete(h_pose_cov); delete(h_pose_cov);
end end
plotCamera(poses{i}, 2);
gRp = poses{i}.rotation().matrix(); % rotation from pose to global gRp = poses{i}.rotation().matrix(); % rotation from pose to global
C = poses{i}.translation().vector(); C = poses{i}.translation().vector();
axisLength = 2; % axisLength = 2;
%
xAxis = C+gRp(:,1)*axisLength; % xAxis = C+gRp(:,1)*axisLength;
L = [C xAxis]'; % L = [C xAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','r'); % line(L(:,1),L(:,2),L(:,3),'Color','r');
%
yAxis = C+gRp(:,2)*axisLength; % yAxis = C+gRp(:,2)*axisLength;
L = [C yAxis]'; % L = [C yAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','g'); % line(L(:,1),L(:,2),L(:,3),'Color','g');
%
zAxis = C+gRp(:,3)*axisLength; % zAxis = C+gRp(:,3)*axisLength;
L = [C zAxis]'; % L = [C zAxis]';
line(L(:,1),L(:,2),L(:,3),'Color','b'); % line(L(:,1),L(:,2),L(:,3),'Color','b');
%
pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame
gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
h_pose_cov = gtsam.covarianceEllipse3D(C,gPp); h_pose_cov = gtsam.covarianceEllipse3D(C,gPp);
@ -114,18 +120,19 @@ end
% wait for two seconds % wait for two seconds
pause(2); pause(2);
% change views % change views angle
for i = 0 : 0.5 : 60 for i = 0 : 0.5 : 60
view([i + 30, i]); view([i + 30, i]);
end end
% camera flying through
%% plot point covariance for i = 1 : posesSize
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
% if exist('h_cylinder', 'var') camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
% delete(h_cylinder); camlight(hlight, 'headlight');
% end
drawnow
end
if ~holdstate if ~holdstate