Clear Filters
Clear Filters

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

2 views (last 30 days)
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?

Accepted Answer

Zheng Dong
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
  6 Comments
cui,xingxing
cui,xingxing on 13 Jan 2023
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
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

Sign in to comment.

More Answers (0)

Products


Release

R2022b

Community Treasure Hunt

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

Start Hunting!