gtsam - bearing only slam landmark initialization

Question

I am using gtsam in a bearing only approach to slam for college. My question is regarding the initializaton of the landmarks. My teacher said there was something called 'smart factors' that I could use instead of normal factors for the landmarks, and as such, I wouldn't need to initialize them, and then in the optimization phase the landmarks positions and associated noise would come out but I don't seem to find anything like this. Any help would be greatly apreciated

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: currently the code does not run and like expected the error is from the un-initialized landmark poses. ERROR: Exception from gtsam: Attempting to retrieve the key "l1", which does not exist in the Values.

Answer 1

I've since found out there are smart factor classes on the current unstable folders of the project, but I was unable to use them even though my installation was successful. My solution was : the first time I saw a new landmark I would initialize it with a random range and the provided bearing. (range sensor noise would be very big)