bug: factorGraph does not work for relative measurement angles

2023 1 5
1 个回答
回答已采纳
更新时间：2023 1 11
44 次查看（30 天）
Ran in:

0 个投票

Ran in:
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 个评论
显示 1更早的评论 隐藏 1更早的评论

xingxingcui
xingxingcui 2023-1-6
编辑：xingxingcui 2023-1-6
Ran in:
Ran in:
I understand that the factorGraph in matlab is derived from the literature "Dellaert, Frank. Factor graphs and GTSAM: A Hands-On Introduction. Georgia: Georgia Tech, September, 2012”,
The online project GTSAM is described in section 4.2 here, and is very similar to the example I gave above, where according to the description in section 4.2 "Using the MATLAB Interface", the relative angle of rotation is also pi/2, This means that my example above looks correct, but it is still unclear where the problem lies.
%% factorGraph优化
% 由于factorTwoPoseSE2暂无法指定起始点，故用se3来进行
fg = factorGraph();
% pf = factorPoseSE3Prior(0,Measurement=[0,0,0, 1,0,0,0]);
% addFactor(fg,pf);
fctr = factorTwoPoseSE3([0 1],Measurement=[0 0 0 1 0 0 0]);
addFactor(fg,fctr);
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
relZ = pi/2;
relY = 0;
relX = 0;
relPose = [1,0,0,eul2quat([relZ,relY,relX]);
0,1,0,eul2quat([relZ,relY,relX]);
-1,0,0,eul2quat([relZ,relY,relX]);
0,-.8,0,eul2quat([relZ,relY,relX])];
% relPose = diff(abspos);
% relPose = [relPose;
% abspos(1,:)-abspos(end,:)];
zeroQuat = [1 0 0 0];
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.3932 -0.3266 0 0.9992 0 0 -0.0411 0.6035 -0.4100 0 1.0000 0 0 0.0076 0.5882 0.5885 0 0.9984 0 0 0.0564 -0.4054 0.4747 0 1.0000 0 0 0.0076
theta = quat2eul(newNodes(:,4:end))
theta = 4×3
-0.0822 0 0 0.0153 0 0 0.1128 0 0 0.0153 0 0
D2 = pdist(newNodes(:,1:2))
D2 = 1×6
1.0001 1.3418 0.8013 0.9987 1.3418 1.0001
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')
as we can see,the initial node coordinates cannot be specified by priors, and the absolute angular orientation theta of the 4 nodes is also a problem
Zheng Dong
Zheng Dong 2023-1-9
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

请先登录，再进行评论。

请先登录，再回答此问题。

 采纳的回答

Zheng Dong
Zheng Dong 2023-1-10

0 个投票

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 个评论
显示 -1更早的评论 隐藏 -1更早的评论

xingxingcui
xingxingcui 2023-1-11
编辑：xingxingcui 2023-1-11
"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

请先登录，再进行评论。

更多回答（0 个）

类别

在 帮助中心 和 File Exchange 中查找有关 Localization Algorithms 的更多信息

产品

版本

R2022b

标签

Community Treasure Hunt

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

Start Hunting!

Translated by