Error using rigid3d after repeated execution in visual odometry
4 次查看(过去 30 天)
显示 更早的评论
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 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!