bug: factorGraph does not work for relative measurement angles

2 views (last 30 days)
I tried to add relative pose measurements of 2 adjacent nodes using factorGraph's object function factorTwoPoseSE3, but no matter how much I should modify the relative angle “relZ”,“relY”,“relX” any variables(i.e. the number of quaternions), the optimized result graph remains the same, which is apparently a big problem?
If this is not the case, how do I modify the code to achieve the correct optimisation result that I want?
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.
% 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
% no matter how much I should modify these relZ,relY,relX any values, the result newNodes does't change???
relZ = pi/2;
relY = 0;
relX = 0;
relPose = [1,0,0,eul2quat([relZ,relY,relX]);
1,0,0,eul2quat([relZ,relY,relX]);
1,0,0,eul2quat([relZ,relY,relX]);
0.8,0,0,eul2quat([relZ,relY,relX])];
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 0 1.0000 0 0 0 0.0500 0 0 1.0000 0 0 0 0.1000 0 0 1.0000 0 0 0 0.1500 0 0 1.0000 0 0 0
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')
----------------------------------
Additionly, according to the factorTwoPoseSE3 official documentation "Measured relative pose, specified as a seven-element row vector of the form [dx dy dz dqw dqx dqy dqz]. dx, dy, and dz are the change in position in x, y, and z respectively. dqw, dqx, dqy, and dqz are the change in quaternion rotation in w, x, y, and z respectively."
So, I have made the following changes to the relpos and it seems that the optimisation graphs are not correct, which is very confusing!
relPose = diff(abspos);
relPose = [relPose;
abspos(1,:)-abspos(end,:)];
  3 Comments
Zheng Dong
Zheng Dong on 9 Jan 2023
Hi Cui,
The measurement is relative to the ego frame not the world frame, which is the same as the relPose input in poseGraph. As for the result you got for the SE3 case, we are investigating on it now. We will add answers below if we get any updates.
Thanks,
Zheng

Sign in to comment.

Accepted Answer

Zheng Dong
Zheng Dong on 10 Jan 2023
Hi Cui,
The measurement is relative to the ego frame not the world frame, so your first approach is on the right track. However, you still need to give an initial guess for the node in the factorGraph. As for your example, you need to do nodeState(fg, 1:4, initial_guess) before optimization. poseGraph will automatically accumulate the relPose as initial guess for each node when you do addRelativePose, however, factorGraph will not. So you need to do it by yourself. Hope it helps.
Thanks,
Zheng
  1 Comment
cui,xingxing
cui,xingxing on 11 Jan 2023
Edited: cui,xingxing on 11 Jan 2023
"The measurement is relative to the ego frame not the world frame, so your first approach is on the right track."
is this a refer to this link example1 code? if it is, but it use poseGraph ,not fatcorGraph.If not, the first code in this example problem is indeed relative to the ego frame, but no matter how I set the angle value, I still won't get the desired pose ([x,y,z,q,X,Y,Z] format or [x,y,theta] format, this link example1 code result).
nodeState is a good function for both setting and getting the pose of the factor graph, which might solve the initial position setting I was using with factorGraph.
In the comments section of this example I have tried my best to change the measurement to try to optimise the pose diagram, and the shape results look correct, but just short of the correctness of the angle. However, this comment is using a relative world frame!(confuse me point)
Anyway, it would be more convincing if you could give the code for correctness based on my example.@Zheng Dong

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!