清空筛选条件
清空筛选条件

Error using rigid3d after repeated execution in visual odometry

4 次查看（过去 30 天）
Robin Pape
Robin Pape 2021-2-17
评论： Robin Pape 2021-3-2
Hi everyone!
I have a problem using the built-in function rigid3d in the context of visual odometry, because it fails at some point after a couple of hundred executions.
What I do: I estimate a relative camera pose between two successive image frames, and to get the world camera pose, I add them up every time. This is the code that is executed with every frame:
% estimate essential matrix to get an estimate for relative motion between two
% perspectives
[E, inliersEssential] = estimateEssentialMatrix(matchedPointsPrevious, matchedPoints, this.intrinsics, 'MaxNumTrials', 500, 'Confidence', 99);
% estimate relative camera pose
[relativeOrientation, relativeLocation] = relativeCameraPose(E, this.intrinsics, matchedPointsPrevious(inliersEssential, :), matchedPoints(inliersEssential, :));
% estimate world camera pose by adding the poses incrementally
worldOrientation = worldOrientationPrevious(:,:,1) * relativeOrientation(:,:,1);
worldLocation = worldLocationPrevious(1,:) + (relativeLocation(1,:) * worldOrientation(:,:,1));
% create a rigid transformation for further use
pose = rigid3d(worldOrientation(:,:,1), worldLocation(1,:));
This works until after a couple of hundred images, the function rigid3d fails because the transformation is not rigid anymore.
In rigid3d.m, line 375 following:
function isRigid = isTransformationMatrixRigid(T)
%This is looser than affine3d/isRigid, and is necessary for
%chained transformation operations which are common in
%registration workflows.
rot = T(1:3,1:3);
singularValues = svd( rot );
isRigid = max(singularValues) - min(singularValues) < 1000*eps(max(singularValues(:)));
isRigid = isRigid && abs(det(rot)-1) < 1000*eps(class(rot)); % <--- this line fails
end
In the last line, the condition is not true anymore, i. e. the determinant of the rotation matrix is just slightly bigger than one:
K>> abs(det(rot)-1)
ans =
2.2293e-13
K>> 1000*eps(class(rot))
ans =
2.2204e-13
I think the problem is probably computational precision because of the ongoing incremental multiplication of the orientations.
Any idea how I can avoid this error? Thanks!

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

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Point Cloud Processing 的更多信息

标签

产品


版本

R2020a

Community Treasure Hunt

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

Start Hunting!

Translated by