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

15 次查看(过去 30 天)
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!

采纳的回答

cui,xingxing
cui,xingxing 2022-12-5
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.
  5 个评论
Zheng Dong
Zheng Dong 2022-12-8
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

请先登录,再进行评论。

更多回答(1 个)

Akshai Manchana
Akshai Manchana 2024-3-11,8:24
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

Community Treasure Hunt

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

Start Hunting!

Translated by