60 lines
1.4 KiB
Matlab
60 lines
1.4 KiB
Matlab
|
function VisualISAMPlot(truth, data, isam, result, options)
|
||
|
% VisualISAMPlot: plot current state of ISAM2 object
|
||
|
|
% Authors: Duy Nguyen Ta and Frank Dellaert
|
||
|
|
|
||
|
h=gca;
|
||
|
|
cla(h);
|
||
|
|
hold on;
|
||
|
|
|
||
|
|
%% Plot points
|
||
|
|
% Can't use data because current frame might not see all points
|
||
|
|
marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow
|
||
|
|
gtsam.plot3DPoints(result, [], marginals);
|
||
|
|
|
||
|
|
%% Plot cameras
|
||
|
|
import gtsam.*
|
||
|
|
M = 1;
|
||
|
|
while result.exists(symbol('x',M))
|
||
|
|
ii = symbol('x',M);
|
||
|
|
pose_i = result.at(ii);
|
||
|
|
if options.hardConstraint && (M==1)
|
||
|
|
gtsam.plotPose3(pose_i,[],10);
|
||
|
|
else
|
||
|
|
P = marginals.marginalCovariance(ii);
|
||
|
|
gtsam.plotPose3(pose_i,P,10);
|
||
|
|
end
|
||
|
|
if options.drawTruePoses % show ground truth
|
||
|
|
gtsam.plotPose3(truth.cameras{M}.pose,[],10);
|
||
|
|
end
|
||
|
|
|
||
|
|
M = M + options.cameraInterval;
|
||
|
|
end
|
||
|
|
|
||
|
|
%% draw
|
||
|
|
axis([-40 40 -40 40 -10 20]);axis equal
|
||
|
|
view(3)
|
||
|
|
colormap('hot')
|
||
|
|
drawnow
|
||
|
|
|
||
|
|
%% do various optional things
|
||
|
|
|
||
|
|
if options.saveFigures
|
||
|
|
fig2 = figure('visible','off');
|
||
|
|
newax = copyobj(h,fig2);
|
||
|
|
colormap(fig2,'hot');
|
||
|
|
set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]);
|
||
|
|
print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M));
|
||
|
|
end
|
||
|
|
|
||
|
|
if options.saveDotFiles
|
||
|
|
isam.saveGraph(sprintf('VisualiSAM%03d.dot',M));
|
||
|
|
end
|
||
|
|
|
||
|
|
if options.saveDotFile
|
||
|
|
isam.saveGraph(sprintf('VisualiSAM.dot'));
|
||
|
|
end
|
||
|
|
|
||
|
|
if options.printStats
|
||
|
|
isam.printStats();
|
||
|
|
end
|