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

7 views (last 30 days)
cui on 4 Jan 2023
Commented: Zheng Dong on 13 Jan 2023
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();
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]);
nodeID = [1 2];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(1,:));% odometry
nodeID = [2 3];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(2,:));% odometry
nodeID = [3 4];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(3,:));% odometry
nodeID = [4 1];
f = factorTwoPoseSE3(nodeID,Measurement=relPose(end,:));% loop closure
% 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???
nodeID = [1 2];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(1,:));% odometry
nodeID = [2 3];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(2,:));% odometry
nodeID = [3 4];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(3,:));% odometry
nodeID = [4 1];
f = factorTwoPoseSE2(nodeID,Measurement=relPose(end,:));% loop closure
% 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？

Zheng Dong on 9 Jan 2023
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
Zheng Dong on 13 Jan 2023
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

### Categories

Find more on Localization Algorithms in Help Center and File Exchange

R2022b

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!