42 lines
1.5 KiB
Matlab
42 lines
1.5 KiB
Matlab
function [ isam, results ] = VisualISAMInitialize( data, reorderInterval )
|
|
%VISUALISAMINITIALIZE Initialize the first two poses and update ISAM
|
|
if (nargin<2), reorderInterval = 1; end
|
|
isam = visualSLAMISAM(reorderInterval);
|
|
|
|
%% Add new factors
|
|
newFactors = visualSLAMGraph;
|
|
newFactors.addPosePrior(symbol('x',1), data.cameras{1}.pose, data.posePriorNoise);
|
|
newFactors.addPointPrior(symbol('l',1), data.points{1}, data.pointPriorNoise);
|
|
|
|
odometry = data.cameras{1}.pose().between(data.cameras{2}.pose());
|
|
newFactors.addOdometry(symbol('x',1), symbol('x',2), odometry, data.odometryNoise);
|
|
|
|
for i=1:2
|
|
for j=1:size(data.points,2)
|
|
zij = data.cameras{i}.project(data.points{j});
|
|
newFactors.addMeasurement(zij, data.measurementNoise, symbol('x',i), symbol('l',j), data.K);
|
|
end
|
|
end
|
|
|
|
%% Initial estimats for new variables
|
|
initials = visualSLAMValues;
|
|
initials.insertPose(symbol('x',1), data.cameras{1}.pose);
|
|
initials.insertPose(symbol('x',2), data.cameras{2}.pose);
|
|
for j=1:size(data.points,2)
|
|
initials.insertPoint(symbol('l',j), data.points{j});
|
|
end
|
|
|
|
%% Update ISAM
|
|
isam.update(newFactors, initials);
|
|
results.frame_i = 2;
|
|
results.estimates = isam.estimate();
|
|
for i=1:2
|
|
results.Pposes{i} = isam.marginalCovariance(symbol('x',i));
|
|
end
|
|
|
|
for j=1:size(data.points,2)
|
|
results.Ppoints{j} = isam.marginalCovariance(symbol('l',j));
|
|
end
|
|
end
|
|
|