bug: factorGraph does not work for relative measurement angles

Question

cui,xingxing 2023-1-5

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1888682-bug-factorgraph-does-not-work-for-relative-measurement-angles

编辑： cui,xingxing 2023-1-11

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]);

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更早的评论

cui,xingxing 2023-1-6

编辑：cui,xingxing 2023-1-6

在 MATLAB Online 中打开

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 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

cui,xingxing 2023-1-11

@Zheng Dong Looking forward to your code demonstration

请先登录，再进行评论。

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

Answer 1

Zheng Dong 2023-1-10

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1888682-bug-factorgraph-does-not-work-for-relative-measurement-angles#answer_1145670

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更早的评论

cui,xingxing 2023-1-11

编辑：cui,xingxing 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

请先登录，再进行评论。

bug: factorGraph does not work for relative measurement angles

3 个评论
显示 1更早的评论隐藏 1更早的评论

采纳的回答

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

bug: factorGraph does not work for relative measurement angles

3 个评论 显示 1更早的评论隐藏 1更早的评论

采纳的回答

1 个评论 显示 -1更早的评论隐藏 -1更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

3 个评论
显示 1更早的评论隐藏 1更早的评论

1 个评论
显示 -1更早的评论隐藏 -1更早的评论