gtsam -只包含slam地标初始化

Question

我正在使用gtsam的轴承唯一的方法，为大学的slam。我的问题是关于地标的草签化。我的老师说，有一种叫做“智能因素”的东西，我可以用它来代替普通的因素，因此，我不需要初始化它们，然后在优化阶段，地标位置和相关的噪音就会出现，但我似乎没有找到这样的东西。任何帮助都会得到极大的认可。

import gtsam.*

% load simulation
load sim.mat


[~, ntimes] = size(utrue);

% create graph
graph = NonlinearFactorGraph;
% define odometry and bearing sensor noise
odometryNoise = noiseModel.Diagonal.Sigmas([SIGMA_G; SIGMA_G; SIGMA_R]);
bearingNoise = noiseModel.Diagonal.Sigmas(GSIGMA_BEARING);
% define initial estimate
initialEstimate = Values;

% Initial robot pose
initialPose = Pose2(xtrue(1,1), xtrue(2,1), xtrue(3,1));
graph.add(PriorFactorPose2(symbol('x',1), initialPose, odometryNoise));
%
dv = DT*WHEEL_RADIUS*utrue(1,1);
u(1) = dv*cos(initialPose.theta + utrue(2,1));
u(2) = dv*sin(initialPose.theta + utrue(2,1));
u(3) = dv*sin(utrue(2,1))/WHEEL_BASE;
odometry = Pose2(u(1), u(2), u(3));
graph.add(BetweenFactorPose2(symbol('x',1), symbol('x',2), odometry, odometryNoise));
initialEstimate.insert(symbol('x',1), initialPose);


last_pose = initialPose;
% SLAM cycle
for t=2:ntimes
    % odometry values
    v = utrue(1,t);
    w = utrue(2,t);

    dv = DT*WHEEL_RADIUS*v;
    u(1) = dv*cos(last_pose.theta + w);
    u(2) = dv*sin(last_pose.theta + w);
    u(3) = dv*sin(w)/WHEEL_BASE; 
    
    odometry = Pose2(u(1), u(2), u(3));
    graph.add(BetweenFactorPose2(symbol('x',t-1), symbol('x',t), odometry, odometryNoise));
    
 
    pose = Pose2(last_pose.x + u(1), last_pose.y + u(2), last_pose.theta + u(3));
    initialEstimate.insert(symbol('x',t), pose);
    last_pose = pose;
    
    lm_indx = obs(3,t);
    if lm_indx ~= 0
        % laser sensor ONLY with bearing
        obs_bearing = obs(2,t);
        graph.add(BearingFactor2D(symbol('x',t), symbol('l',lm_indx), Rot2(obs_bearing), bearingNoise));
    end
end



optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely();


% Plot Covariance Ellipses
figure(2)
cla;hold on
marginals = Marginals(graph, result);
plot2DTrajectory(result, 'g', marginals);
plot2DPoints(result, 'b', marginals);
plot(xtrue(1,:),xtrue(2,:),'r');
axis tight
axis equal
view(2)


%figure(3)

% print
graph.print(sprintf('\nFull graph:\n'));
result.print(sprintf('\nFinal result:\n'));

PS:目前，代码没有运行，并且和预期的一样，错误来自未初始化的里程碑式。错误：Exception from gtsam: Attempting to retrieve the key "l1", which does not exist in the Values.

Answer 1

从那时起，我就发现在项目当前的不稳定文件夹中有智能因子类，但即使安装成功，我也无法使用它们。我的解决方案是:当我第一次看到一个新的地标时，我会用一个随机范围和提供的方位初始化它。(距离传感器的噪音会很大)