Error using rigid3d after repeated execution in visual odometry

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!

[lin xinyu]

The same problem. Any solution?

[Robin Pape]

Unfortunately, I haven't found a solution to this yet