bug: factorGraph does not work for relative measurement angles

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

[cui,xingxing]

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]

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]

@Zheng Dong Looking forward to your code demonstration

Answer 1

[Zheng Dong]

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

[cui,xingxing]

"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