BearingRangeFactor2D between Poses

With the advent of "Values" being able to take on any type, it appears that BearingRangeFactor2D should be able to take two poses as input. However, this appears to cause a factor graph to become unsolvable due to a conflict in type:

"Attempting to retrieve value with key "x2", type stored in Values is N5gtsam12GenericValueINS_5Pose2EEE but requested type was N5gtsam6Point2E"

my code is shown below, and is also an attachment along with the error log (using MATLAB 2017A)

close all; clear all; clc

%%

diary('log.txt') addpath('/usr/local/gtsam_toolbox/') import gtsam.*

%% Create keys for variables i1 = symbol('x',1); i2 = symbol('x',2);

%% Create graph container and add factors to it graph = NonlinearFactorGraph;

%% Add prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); graph.add(PriorFactorPose2(i1, priorMean, priorNoise));

priorMean2 = Pose2(2.0, 0.0, 0.0); % prior at origin graph.add(PriorFactorPose2(i2, priorMean2, priorNoise));

%% Add odometry odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));

%% Add bearing/range measurement factors degrees = pi/180; brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); graph.add(BearingRangeFactor2D(i1, i2, Rot2(0*degrees), 2, brNoise));

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

%% Initialize to noisy points initialEstimate = Values; initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2));

initialEstimate.print(sprintf('\nInitial estimate:\n'));

%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd optimizer = DoglegOptimizer(graph, initialEstimate); result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); diary off

Comments (3)