poseGraph: Why is the node trajectory graph obtained by poseGraph not the same as the relative rigidtform2d() calculate result?

When I try to add relative pose "measurement" to the "poseGraph" object one by one using "addRelativePose", there is a big difference between the absolute pose position coordinates before the global optimized pose and the actual calculated pose position, what is the problem?

In the following procedure I have given a series of values for the relative attitude "measurement" (see attachment) and compared the results with the calculation program.

%% construct poseGraph 
load measurements.mat
pg = poseGraph();
calAbsPose = rigidtform2d();
absPoseCum = [];

numsNodes = numel(relPoses);
for i = 1:numsNodes
    currRelPose = relPoses(i);
    measurement = [currRelPose.A(1,3),currRelPose.A(2,3),deg2rad(currRelPose.RotationAngle)];% Relative measurement pose, [x,y,theta] format
    addRelativePose(pg,measurement);

    calAbsPose = rigidtform2d(currRelPose.A*calAbsPose.A);% Calculated absolute pose
    absPoseCum = [absPoseCum;calAbsPose];
end

%% plot trajectory result
figure;pg.show();title("poseGraph node trajectory")

traj = [];
for i = 1:numel(absPoseCum)
    A = absPoseCum(i).A;
    traj = [traj;A(1,3),A(2,3)];
end
figure;plot(traj(:,1),traj(:,2),'k-');axis equal;grid on;
title("calculate trajectory")

As you can see from these 2 drawings, the shapes are similar but the coordinates are vastly different, which is the right case and how to fix it, your answer would be greatly appreciate!

Answer 1

[cui,xingxing]

After careful analysis, I found the problem. The second input parameter of the addRelativePose function, "measurement", can easily be misinterpreted as a relative value in the global world coordinate system, but in fact it is a relative observed value relative to the current ego, as shown in the following diagram, with the pentagram being the observed target and theta being the observed angle (note that theta is in radians).

%% poseGraph 姿态轨迹图问题
load measurements.mat
pg = poseGraph();
preAbsPose = rigidtform2d();
absPoseCum = [];

numsNodes = numel(relPoses);
for i = 1:numsNodes
    currRelPose = relPoses(i);% note:这里的相对姿态数值单位是指参考的基准是世界坐标系（一般不变）
    currAbsPose = rigidtform2d(currRelPose.A*preAbsPose.A);% Calculated absolute pose
    absPoseCum = [absPoseCum;preAbsPose];

    %     measurement = [currRelPose.A(1,3),currRelPose.A(2,3),deg2rad(currRelPose.RotationAngle)];
    relR = preAbsPose.R'*currAbsPose.R; % https://ww2.mathworks.cn/matlabcentral/answers/1720045-how-to-get-the-relative-camera-pose-to-another-camera-pose
    relT = preAbsPose.R'*(currAbsPose.Translation'-preAbsPose.Translation');
    eul = rotm2eul(blkdiag(relR,1));
    measurement = [relT(1),relT(2),eul(1)];% Relative measurement pose, [x,y,theta] format，theta is in radians.
    addRelativePose(pg,measurement);
    preAbsPose = currAbsPose;
end

%% plot trajectory result
figure;pg.show();title("poseGraph node trajectory")

traj = [];
for i = 1:numel(absPoseCum)
    A = absPoseCum(i).A;
    traj = [traj;A(1,3),A(2,3)];
end
figure;plot(traj(:,1),traj(:,2),'k-');axis equal;grid on;
title("calculate trajectory")

Now they are the same.

-------------------------Off-topic interlude, 2024-------------------------------

I am currently looking for a job in the field of CV algorithm development, based in Shenzhen, Guangdong, China,or a remote support position. I would be very grateful if anyone is willing to offer me a job or make a recommendation. My preliminary resume can be found at: https://cuixing158.github.io/about/ . Thank you!

Email: [e-mail]

[cui,xingxing]

The official addRelativePose example doesn't make it easy to understand how the function works, so here's what I've given that's more relevant, In addition the relative posture addRelativePose can be visualised in the following test experiments

% Designing a positive hexagonal pose trajectory
theta = linspace(0,2*pi,7);
x = cos(theta);
y = sin(theta);
figure;plot(x,y,'ro-');grid on;axis equal;
title("origin simulate trajectory")

%% poseGraph calculate trajectory
pg = poseGraph();
for i = 1:6
    measurement1 = [0.5,sqrt(3)/2,pi/3];
    addRelativePose(pg,measurement1);
end
figure;pg.show();title("poseGraph node trajectory")

Note that the poseGraph origin always starts at (0, 0), which is not a big deal.

[Zheng Dong]

Hi Cui,

Thanks a lot for the investigation and sharing the information with us. I am sorry about the confusion caused by the addRelativePose function. Your conclusion is correct. The relative pose is between the two nodes (the current node and the previously added node) not between the current node and the global world frame. It is mentioned in the doc page, but not clear enough. Thanks a lot for pointing it out and sharing us with the good example. We will definitely improve the documentation soon.

Thanks,

Zheng

[cui,xingxing]

@Zheng Dong In addition to the addRelativePose function, the poseGraph/3D series of object functions examples should be suitably modified to be more reflective of the function mechanics, such as the optimizePoseGraph function, which could provide an example similar to the one below.

Design a planar rectangle with 4 observed absolute coordinates (0,0), (1,0), (1,1), (0,1), and node numbers 1,2,3,4 respectively, in the lastNode 4 observes the relative coordinates of node 1 as (0,-0.2), i.e. the closed loop.

• Exmaple1,relative to global world frame

abspos = [0,0,0;
 1,0,0;
 1,1,0;
 0,1,0;
 0,0.2,0];
relPose = diff(abspos);
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 0 0 1 1 0 0 1 0

figure;show(pg,'IDs','all'); title('loop before')

updatedGp = optimizePoseGraph(pg);
newNodes = nodeEstimates(updatedGp)

newNodes = 4×3

0 0 0 1.0008 -0.0400 0.0200 0.9816 0.9198 0.0400 -0.0168 0.8398 0.0199

figure;show(updatedGp,"IDs","all"); title('loop after')

• Exmaple2,relative to ego world frame

relPose = [1,0,pi/2;
 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('loop before')

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

figure;show(updatedGp,"IDs","all"); title('loop after')

----------------------------------

BTW,Similarly I encountered ambiguity in image retrieval, and I have elaborated on many of the relevant function relationships and class diagram descriptions in this answer, which I hope can be added to the official documentation.

[Zheng Dong]

Hi Cui,

Thanks for the feedback! I agree that the current example for optimizePoseGraph does not include the process about genereating the pose graph. We will consider adding another example including more details for the function based on your feedbacks.

Thanks,

Zheng

[cui,xingxing]

@Zheng Dong,I have a similar problem with factorGraph and would be grateful for a reply.

Answer 2

[Akshai Manchana]

Hi Cui,

One more small issue in the specified fix is the following.

% (From the above fix)
currAbsPose = rigidtform2d(currRelPose.A*preAbsPose.A);

% The actual expectation from pose graph and factor graph
currAbsPose = rigidtform2d(preAbsPose.A*currRelPose.A);

Also newer pose representation objects se2 and se3 can help in converting between pose vector, rigid formats.

(From the above fix)

relR = preAbsPose.R'*currAbsPose.R; % https://ww2.mathworks.cn/matlabcentral/answers/1720045-how-to-get-the-relative-camera-pose-to-another-camera-pose

relT = preAbsPose.R'*(currAbsPose.Translation'-preAbsPose.Translation');

eul = rotm2eul(blkdiag(relR,1));

measurement = [relT(1),relT(2),eul(1)];% Relative measurement pose, [x,y,theta] format，theta is in radians.

This can achieved better using the following.

measurement = se2(preAbsPose.invert().A*currAbsPose.A).xytheta;
currPose  = se2(preAbsPose.A*currRelPose.A).xytheta;

Thanks & Regards

Akshai Manchana

[cui,xingxing]

@Akshai Manchana Thanks for adding to your answer!