2016-06-11 08:48:42 +08:00
|
|
|
%% Add factors for all measurements
|
2016-06-16 03:00:24 +08:00
|
|
|
noise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
|
|
|
|
for i = 1:length(Z),
|
|
|
|
for k = 1:length(Z{i})
|
2016-06-11 08:48:42 +08:00
|
|
|
j = J{i}{k};
|
|
|
|
G.add(GenericProjectionFactorCal3_S2(
|
2016-06-16 03:00:24 +08:00
|
|
|
Z{i}{k}, noise, symbol('x', i), symbol('p', j), K));
|
2016-06-11 08:48:42 +08:00
|
|
|
end
|
|
|
|
end
|