How to create PoseGraph2D with LoopClosure

Question

在 MATLAB Online 中打开

0 个投票

Hello Everyone,

I hope anyone can give me some advice. Facing the problem, that I can't create a 2D-PoseGraph with LoopClosures.

I can fill the pose graph with relative poses iteratively until I want to create loop closures.

In the example code below I just want to prepare a pose graph with 4 nodes. Without loop closures it works fine.

But how can I tell the posegraph-object to create loop closures between node [1,3] and [2,4].

Very,very grateful for any advice. It is my first question in here, so I hope ask in a proper way.

clear all;
clc;
%Create PoseGraph object
poseGraph = robotics.PoseGraph;
%Create information matrix as default
infoMatrix = [ 1 0 0 1 0 1 ];
%Relative Poses
T =  [ 0 4 -90*pi/180 ; 0 4 -90*pi/180 ; 0 4 -90*pi/180 ; 0 4 -90*pi/180 ; 4 4 -90*pi/180 ; 0 4 -90*pi/180]; 
fromNodeID= [ 1 ; 2 ; 3 ; 4 ; 1 ; 2];
toNodeID= [ 2 ; 3 ; 4 ; 1 ; 3 ; 4];
for i=1:size(T,1)
      relPose = [T(i,1) , T(i,2) , T(i,3)];
      
      fromNodeID = fromNodeID(i,1);
      toNodeID = toNodeID(i,1);
       
      addRelativePose(poseGraph,relPose,infoMatrix,fromNodeID,toNodeID);
end
disp(poseGraph);
show(poseGraph);