Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

BearingRangeFactor #2

Open
laithrns opened this issue Jul 27, 2016 · 0 comments
Open

BearingRangeFactor #2

laithrns opened this issue Jul 27, 2016 · 0 comments

Comments

@laithrns
Copy link

Below I describe the issue we encounter when using BearingRangeFactor2D from the GTSAM pre-compiled toolbox to describe an inter-vehicle bearing/range measurement.

Within the current GTSAM structure I think the BearingRangeFactor2D is designed to construct factor graphs for landmarks-based SLAM i.e. to describe bearing/range measurements to a landmark to solve the SLAM problem.

The BearingRangeFactor2D constructor is defined as
BearingRangeFactor2D(size_t poseKey, size_t pointKey, Rot2 measuredBearing, double measuredRange, Base noiseModel)

As you will notice that the second argument is of type pointKey which is given by (x,y) -to describe the location of a landmark or feature- while a poseKey is defined by (x,y, theta). When using the BearingRangeFactor2D as shown in the attached pdf, it is required to provide an initial guess or estimate of the location of the landmark before executing the optimization routine. Below a piece of MATLAB script excerpt from GTSAM provided PlanarSLAMExample to describe a facotr graph for agent a and landmarks l1 and l2 as shown in the attached pdf

%-------------------------------------------------SingleAgent----------------------------------------------------------%
% Create keys for variables
% Create (instantiate) factor graph
% Add priors

%% Add odometry
graph.add(BetweenFactorPose2(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(x2, x3, odometry, odometryNoise));

%% Add bearing/range measurement factors
graph.add(BearingRangeFactor2D(x1, l1, Rot2(45_degrees),2, brNoise));
graph.add(BearingRangeFactor2D(x2, l1, Rot2(90_degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(x3, l2, Rot2(90*degrees), 2, brNoise));

%% Initialize to noisy points
initialEstimate = Values;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(0, 0)); % A required initial guess of landmark of type Point2
initialEstimate.insert(l2, Point2(0, 0)); % A required initial guess of landmark of type Point2
%% Optimize using Levenberg-Marquardt optimization
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); % initialEstimate must include a guess of landmarks
result = optimizer.optimizeSafely(); % result returns an optimized solution that includes an estimate of the landmark locations as what an EKF-SLAM would do.

%--------------------------------------------------------------------------------------------------------------------------%

When using the BearingRangeFactor2D to describe an inter-vehicle bearing/range measurement as shown in the attached pdf between agent 1 and 2, we run into the problem of having a second guess of the vehicle been measured. The below MATLAB script describes this issue

%---------------------------------------MultipleAgents-----------------------------------------------------------------%

%% Add odometry
graph.add(BetweenFactorPose2(a1, a2, odometry, odometryNoise)); % odometry factors for agent a
graph.add(BetweenFactorPose2(a2, a3, odometry, odometryNoise));

graph.add(BetweenFactorPose2(a1, a2, odometry, odometryNoise)); % odometry factors for agent b
graph.add(BetweenFactorPose2(a2, a3, odometry, odometryNoise));

%% Add bearing/range measurement factors
graph.add(BearingRangeFactor2D(a2, b2, Rot2(90*degrees), 2, brNoise));

%% Initialize to noisy points
initialEstimate = Values;
initialEstimate.insert(a1, Pose2(0.5, 0.0, 0.2)); % initial guesses for agent's a
initialEstimate.insert(a2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(a3, Pose2(4.1, 0.1, 0.1));

initialEstimate.insert(b1, Pose2(0.5, 0.0, 0.2)); % initial guesses for agent's b
initialEstimate.insert(b2, Pose2(0.3, 0.1,-0.2)); % <-------------------- 1st guess of type Pose
initialEstimate.insert(b3, Pose2(0.1, 0.1, 0.1));

initialEstimate.insert(a2, Point2(0, 0)); % <------------- 2nd guess of agent's b at b2 of type Point2

% ---------------------------------------------------------------------------------------------------------%

Work around (things we tried) & observations:

  1. I used the RangeFactorPose2 to describe range measurements between agents and it works fine, the optimizer does not require an initial guess. A look into the C++ source code, I noticed it is instantiated from the same base class as the BearingRangeFactor2D (C++ class). The second argument of the RangeFacotrPose2 is of type Point2 as that of BearingRangeFactor2D. Which tells me that I may need to look into the optimizer that is using them.

  2. A way to work around it, I have tried to create a virtual "landmark" that lies on the top of the agent i.e. by using a BetweenFactor between the agent say b at b2 and a virtual landmark say lb2 with odometry of zeros (0,0,0) and zero odometry noise. With that I have assumed when optimized the result location of lb2 will be on the top of b2 as defined by the ''BetweenFactor'', but that was not the case. lb2 has a separate location and I think it still considered as a landmark close to the agent b at b2.

bearingrangefactor2d

Regards,
Laith

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant