How to create PoseGraph2D with LoopClosure

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);

Answer 1

[Young Lin]

check this out addrelativepose.

loop closure and relative pose between contigeous frames are just constraint to graph.