factorGraph:Unclear document function factorTwoPoseSE3/SE2 with input parameter 'Measurement'

I figured out some time ago that a similar function poseGraph's input parameter 'Measurement' could be relative to the world frame or relative to the ego frame, but recently using factorGraph was once again confusing, and the documentation was not clear. Below I have done the same 3 simple sets of experiments(examples) for comparison, why don't the latter 2 example methods factorGraph get the results of the first method?

Also why is there no factorPoseSE2Prior function to set the starting pose? However, there is factorPoseSE3Prior to set the starting pose, which is strange!

Design a plane rectangle, its observed 4 absolute coordinate points (0,0), (1,0), (1,1), (0,1), (0,1), node (node) number 1,2,3,4, respectively, in the last node 4 observed node 1 relative coordinates for (0.8,0), that is, closed loop , you can see that the last node error is 0.2. that is, this pose graph contains 4 edges, The first 3 of these are odometers and the last one is a closed loop edge.

Example1 , use poseGraph optimize

% define real global world pose
% abspos = [0,0,0;
%     1,0,90*pi/180;
%     1,1,180*pi/180;
%     0,1,270*pi/180];

% relPose = diff(abspos);% relative to world frame!
% relPose = [relPose;0,0.2,90*pi/180];
relPose = [1,0,pi/2; % Note that the third column refers to the angle being a turn relative to itself,not relative to world frame!
    1,0,pi/2;
    1,0,pi/2;
    0.8,0,pi/2];
pg = poseGraph();

addRelativePose(pg,relPose(1,:),[],1,2);% odometry
addRelativePose(pg,relPose(2,:),[],2,3);% odometry
addRelativePose(pg,relPose(3,:),[],3,4);% odometry
[nodePair,edgeID] = addRelativePose(pg,relPose(end,:),[],4,1);% loop closure

oldNodes = nodeEstimates(pg)

oldNodes = 4×3

0 0 0 1.0000 0 1.5708 1.0000 1.0000 3.1416 0 1.0000 -1.5708

figure;show(pg,'IDs','all'); title('before poseGraph optimize')

updatedGp = optimizePoseGraph(pg);
newNodes = nodeEstimates(updatedGp)

newNodes = 4×3

0 0 0 1.0008 -0.0400 1.5908 0.9816 0.9198 -3.1016 -0.0168 0.8398 -1.5509

D1 = pdist(newNodes(:,1:2));% aim to compare optimized graph result later
figure;show(updatedGp,"IDs","all"); title('after poseGraph optimize')

Example2 , use factorGraph 3D optimize

Since factorTwoPoseSE2 cannot specify a starting point for the time being, se3 is used for this purpose.

abspos = [0,0,0,eul2quat([0,0,0],"ZYX"); % rotate 0 about Z axis
    1,0,0,eul2quat([pi/2,0,0],"ZYX");    % rotate 90*pi/180 about Z axis
    1,1,0,eul2quat([pi,0,0],"ZYX");      % rotate 180*pi/180 about Z axis
    0,1,0,eul2quat([3*pi/2,0,0],"ZYX")]; % rotate 270*pi/180 about Z axis

% relPose = diff(abspos);
% relPose = [relPose;
%     [0,-0.8,0,abspos(1,4:end)-abspos(end,4:end)]];% 0,-0.8,0,eul2quat([0,0,0],"ZYX")
relPose = [1,0,0,eul2quat([pi/2,0,0]); % relative to world frame! [x,y,z,c,X,Y,Z] format
    0,1,0,eul2quat([pi/2,0,0]);
    -1,0,0,eul2quat([pi/2,0,0]);
    0,-0.8,0,eul2quat([-3*pi/2,0,0])];

% relQuat = eul2quat([pi/2,0,0],"ZYX");
% relPose = [1,0,0,relQuat; % relative to ego frame，[x,y,z,c,X,Y,Z] format
%     1,0,0,relQuat;
%     1,0,0,relQuat;
%     0.8,0,0,relQuat];
fg = factorGraph(); 

pf = factorPoseSE3Prior(1,Measurement=[0,0,0, 1,0,0,0]);
addFactor(fg,pf);

nodeID = [1 2];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(1,:));% odometry
addFactor(fg,f);

nodeID = [2 3];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(2,:));% odometry
addFactor(fg,f);

nodeID = [3 4];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(3,:));% odometry
addFactor(fg,f);

nodeID = [4 1];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(end,:));% loop closure
addFactor(fg,f);

% optimize
optns = factorGraphSolverOptions();
optimize(fg,optns);

newNodes = [fg.nodeState(1);
    fg.nodeState(2);
    fg.nodeState(3);
    fg.nodeState(4)]

newNodes = 4×7

0.0000 -0.0000 0 1.0000 0 0 -0.0000 1.0001 -0.0013 0 0.9988 0 0 0.0487 0.9029 0.9926 0 0.9953 0 0 0.0974 -0.0780 0.7975 0 0.9988 0 0 0.0487

D2 = pdist(newNodes(:,1:2));% aim to compare optimized graph result later
figure;
plot(newNodes(:,1),newNodes(:,2),'b-',Marker='.')
hold on;grid on;
plot(newNodes([4,1],1),newNodes([4,1],2),'r-',Marker='.')
text(newNodes(:,1),newNodes(:,2),string(1:4))
title('after factorGraph(3D) optimize')

Example3 , use factorGraph 2D optimize

No intruduce build-in factorPoseSE2Prior function??? optimize result not correct？

abspos = [0,0,0;   % rotate 0 about Z axis
    1,0,pi/2;      % rotate 90*pi/180 about Z axis
    1,1,pi;        % rotate 180*pi/180 about Z axis
    0,1,3*pi/2];   % rotate 270*pi/180 about Z axis

relPose = diff(abspos);
relPose = [relPose;
    0,-0.8,-3*pi/2];

fg = factorGraph(); 

% pf = factorPoseSE2Prior(1,Measurement=[0,0,0]); % can't define origin???
% addFactor(fg,pf);

nodeID = [1 2];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(1,:));% odometry
addFactor(fg,f);

nodeID = [2 3];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(2,:));% odometry
addFactor(fg,f);

nodeID = [3 4];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(3,:));% odometry
addFactor(fg,f);

nodeID = [4 1];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(end,:));% loop closure
addFactor(fg,f);

% optimize
optns = factorGraphSolverOptions();
optimize(fg,optns);

newNodes = [fg.nodeState(1);
    fg.nodeState(2);
    fg.nodeState(3);
    fg.nodeState(4)]

newNodes = 4×3

-0.4995 -0.4200 -0.0200 0.4995 -0.4800 0.0000 0.4995 0.4800 0.0200 -0.4995 0.4200 0.0000

D3 = pdist(newNodes(:,1:2));% aim to compare optimized graph result later
figure;
plot(newNodes(:,1),newNodes(:,2),'b-',Marker='.')
hold on;grid on;
plot(newNodes([4,1],1),newNodes([4,1],2),'r-',Marker='.')
text(newNodes(:,1),newNodes(:,2),string(1:4))
title('after factorGraph(2D) optimize')

You can see that the angle of the third column of newNodes is lacking! Then, test compare D1 and D2, D1 and D3 is it numercial equal.

eps = 10^(-4);
all(D1-D2<eps)

ans = logical
   0

all(D1-D3<eps)

ans = logical
   0

Excluding the effect of absolute coordinates, they also differ when looking only at the comparison of relative distances! It is therefore very strange that their optimisation results are actually different？

Answer 1

[Zheng Dong]

Hi Cui,

We are actively improving the documentation and I am sorry for the confusion caused. There are the answers to your questions. Firstly, the measurement input for factorTwoPoseSE2 is the same as the relPose for poseGraph, which is relative to ego frame not the world frame. Second, factorPoseSE2Prior will be exposed in the future release, you can fix the first node by fixNode node to realize the same functionality. Third, factorGraph and poseGraph use different solvers, so they mght give different optimized result.

Thanks,

Zheng

[xingxingcui]

"Firstly, the measurement input for factorTwoPoseSE2 is the same as the relPose for poseGraph, which is relative to ego frame not the world frame."

As a matter of common sense, measurement is relative to the ego frame, and I agree with you on this point. However this is not the case with the latest version of R2022b, and apparently the comment part of my code in example 2 above is relative to the ego frame, which I have tried many times, but still no luck!

-----------

"Second, factorPoseSE2Prior will be exposed in the future release, you can fix the first node by fixNode node to realize the same functionality. "

Hopefully factorPoseSE2Prior will be introduced soon to make up the chain of functional completeness.

------------

“Third, factorGraph and poseGraph use different solvers, so they mght give different optimized result.”

I do understand that there are small differences between them, but my example above is so simple and classical that there should be no more errors than can be tolerated（eps = 10^(-4)）

------------

Anyway, it would be more convincing if you could design a program based on each of my 3 examples and then run the code with the results being numerically equivalent.@Zheng Dong

[Zheng Dong]

Hi Cui,

The example workflow will be like:

fg = factorGraph();

measurement = [1 0 pi/2];

ID = generateNodeID(fg,4,"factorTwoPoseSE2");

opts = factorGraphSolverOptions;

fg.addFactor(factorTwoPoseSE2(ID(1:3,:), Measurement = ones(3,1)*measurement));

fg.addFactor(factorTwoPoseSE2([3 0], Measurement = [0.8 0 pi/2])); % loop closure

nodeState(fg, 0:3, [0,0,0;

1,0,pi/2;

1,1,pi;

0,1,3*pi/2]); % initial guess

fixNode(fg, 0); % fix the start point.

optimize(fg,opts)

ids = nodeIDs(fg)

fg_state = nodeState(fg,ids)

Zheng

[xingxingcui]

Unfortunately the above code you provided does not work in the R2022b environment! Also will your future answers ensure reliable and online run results? It's similar to the live run I gave.

[Zheng Dong]

Hi Cui,

fg = factorGraph();

measurement = [1 0 pi/2];

opts = factorGraphSolverOptions;

fg.addFactor(factorTwoPoseSE2([0 1], Measurement = measurement));

fg.addFactor(factorTwoPoseSE2([1 2], Measurement = measurement));

fg.addFactor(factorTwoPoseSE2([2 3], Measurement = measurement));

fg.addFactor(factorTwoPoseSE2([3 0], Measurement = [0.8 0 pi/2])); % loop closure

nodeState(fg, 0, [0,0,0]);

nodeState(fg, 1, [1,0,pi/2]);

nodeState(fg, 2, [1,1,pi]);

nodeState(fg, 3, [0,1,3*pi/2]);

fixNode(fg, 0); % fix the start point.

optimize(fg,opts)

ids = nodeIDs(fg)

fg_state0 = nodeState(fg,0)

fg_state1 = nodeState(fg,1)

fg_state2 = nodeState(fg,2)

fg_state3 = nodeState(fg,3)

I have tested this updated version in R2022b. Please give it a try.

Thanks,

Zheng

[xingxingcui]

Thank you for the last example that worked! I was surprised to find that the factorGraph usage is a bit different from our common thinking habits, after adding the relative pose to addFactor, you still need to set the absolute pose via nodeState to make the result work! Reading the documentation many times didn't guide me properly to proceed down this way.

[Zheng Dong]

Hi Cui,

I am glad it worked! I know that it is not that simple as poseGraph. The main reason is that poseGraph's measurement is always relative pose, and it is easier to give an initial guess based on the relPose. However, for factorGraph, there can be different types of factors relating to one pose node like factorIMU, factorGPS... So it is hard to update the initial guess when you add multiple factors to one pose node. Therefore, we need to give a initial guess for the node. Thanks for bringing this to us. We will continue improving the doc pages.

Thanks,

Zheng