47 lines
1.3 KiB
Matlab
47 lines
1.3 KiB
Matlab
% Christan Potthast
|
|
% create linear factor graph
|
|
|
|
function lfg = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n)
|
|
|
|
m = size(measurements,2);
|
|
|
|
% create linear factor graph
|
|
lfg = GaussianFactorGraph();
|
|
|
|
% Point2 at origin
|
|
origin = Point2;
|
|
|
|
% create prior for initial robot pose
|
|
model0_2 = SharedDiagonal([0.2;0.2]);
|
|
prior = Simulated2DPosePrior(origin,model0_2,1);
|
|
lf = prior.linearize(config);
|
|
lfg.push_back(lf);
|
|
|
|
% add prior for landmarks
|
|
model1000 = SharedDiagonal([1000;1000]);
|
|
for j = 1:n
|
|
prior = Simulated2DPointPrior(origin,model1000,j);
|
|
lf = prior.linearize(config);
|
|
lfg.push_back(lf);
|
|
end
|
|
|
|
% add odometry factors
|
|
odo_model = SharedDiagonal([odo_sigma;odo_sigma]);
|
|
for i = 2 : size(odometry,2)
|
|
odo = Point2(odometry{i}(1),odometry{i}(2));
|
|
nlf = Simulated2DOdometry(odo, odo_model, i-1, i);
|
|
lf = nlf.linearize(config);
|
|
lfg.push_back(lf);
|
|
end
|
|
|
|
% add measurement factors
|
|
measurement_model = SharedDiagonal([measurement_sigma;measurement_sigma]);
|
|
for k = 1 : size(measurements,2)
|
|
measurement = measurements{k};
|
|
point = Point2(measurement.z(1),measurement.z(2));
|
|
nlf = Simulated2DMeasurement(point, measurement_model, measurement.i, measurement.j);
|
|
lf = nlf.linearize(config);
|
|
lfg.push_back(lf);
|
|
end
|
|
|