trinterp() returns invalid rotations in transform matrix

[tweng-bdai]

the trinterp() method does not make sure quaternions are valid before converting them to transforms. It calls qslerp which can sometimes generate invalid quaternions.

Repro:

from spatialmath import SE3

se3_1 = SE3()
se3_1.t = np.array([0.5705748101710814, 0.29623210833184527, 0.10764106509086407])
se3_1.R = np.array([[ 0.2852875203191073  ,  0.9581330588259315  ,
    -0.024332536551692617],
   [ 0.9582072394229962  , -0.28568756930438033 ,
    -0.014882844564011068],
   [-0.021211248608609852, -0.019069722856395098,
    -0.9995931315303468  ]])
assert SE3.isvalid(se3_1.A)

se3_2 = SE3()
se3_2.t = np.array([0.5150284150005691 , 0.25796537207802533, 0.1558725490743694])
se3_2.R = np.array([[ 0.42058255728234184  ,  0.9064420651629983   ,
    -0.038380919906699236 ],
   [ 0.9070822373513454   , -0.4209501599465646   ,
    -0.0016665901233428627],
   [-0.01766712176680449  , -0.0341137119645545   ,
    -0.9992617912561634   ]])
assert SE3.isvalid(se3_2.A)

path_se3 = se3_1.interp(end=se3_2, s=15, shortest=False)
print(path_se3[2])
->   1         0         0         0         
     0         1         0         0         
     0         0         1         0         
     0         0         0         1    
print(path_se3[3])
->   0.3149    0.9487   -0.0275    0.5587    
     0.9489   -0.3153   -0.01222   0.288     
    -0.02027  -0.02225  -0.9995    0.118     
     0         0         0         1   

The interp() method returns an SE3 object that holds the SE3 transformation matrices created from the interpolation:

spatialmath-python/spatialmath/baseposematrix.py

Lines 449 to 455 in 4c68fa9

	# SO(3) or SE(3)
	return self.__class__(
	[
	smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest)
	for _s in s
	]
	)

.

However, there is a validity check in the SE3 object that will turn any invalid transforms into identity matrices.

A possible solution is to modify the trinterp() method to turn all quaternions into unit quaternions before converting them into rotation matrices:

spatialmath-python/spatialmath/base/transforms3d.py

Lines 1697 to 1700 in 4c68fa9

	qr = qslerp(q0, q1, s, shortest=shortest)
	pr = p0 * (1 - s) + s * p1
	return rt2tr(q2r(qr), pr)

.

I am not sure if this is the only location in the spatialmath codebase that would benefit from this change.

[petercorke]

Guys, I think that allowing direct assignment to the R property is a mistake. It allows skewed rotation matrices into the SE(3) matrix which causes all sorts of grief down the track.

The .Rt(R, t) method checks the rotation matrix, but doesn't explicitly normalise. It should, by default.

The given rotation matrices have 17 digits which is full float64 precision but I don't know how they were obtained or how skewed they are. .isvalid() calls ishom() which calls isR with a default tolerance of 20 (in units of eps, that's about +/- one on the least significant digit).

    t = np.array([0.5705748101710814, 0.29623210833184527, 0.10764106509086407])
    R = np.array(
        [
            [0.2852875203191073, 0.9581330588259315, -0.024332536551692617],
            [0.9582072394229962, -0.28568756930438033, -0.014882844564011068],
            [-0.021211248608609852, -0.019069722856395098, -0.9995931315303468],
        ]
    )
    assert isrot(R, check=True, tol=5)
    se3_1 = SE3.Rt(trnorm(R), t)
    assert SE3.isvalid(se3_1.A)

If I apply a more stringent test for orthogonality with the assert isrot line this fails. tol=10 is OK, tol=5 is not. The other matrix has the same issues.

If I add a line

R = trnorm(R)

then the stringent isR test passes, as does the rest of the example.

There is a philosophical issue here. I actually think we should be more stringent in checking rotation matrices into an SO3 or SE3 instance. Then we don't have to do hacky things downstream. The constructor should be a gatekeeper. For UnitQuaternion normalisation is the default action. Unfortunately SO3 and SE3 don't have a norm option, but I think they should. Normalisation is a bit expensive, but for folk who know what they're doing they can set that option to False.

I've seen in older issues where people set the rotation matrix to 6-digit float values and wonder why things don't work. We should protect these people, which in turn is less effort for us longer term.

Another design error is that the tolerance for the isR test should be passed in from .isvalid() rather than defaulting to 20.

Suggested changes:

• SO3 and SE3 constructors should accept a norm argument which defaults to True
• the .Rt pseudo-constructor should accept a norm argument which defaults to True
• delete the R property assignment unless it always does a normalisation of the value first
• .isvalid() should accept a tol argument

I'm happy to put these into a PR.

PS. Remember also that SE3 inherits from a python list so instead of

    for i in range(len(path_se3) - 1):
        assert SE3.isvalid(path_se3[i].A)

        if angle is None:
            angle = path_se3[i].angdist(path_se3[i + 1])
        else:
            test_angle = path_se3[i].angdist(path_se3[i + 1])
            assert abs(test_angle - angle) < 1e-6

you could write

    from itertools import pairwise

    for T1, T2 in pairwise(path_se3):
        assert SE3.isvalid(T1.A)

        if angle is None:
            angle = T1.angdist(T2)
        else:
            test_angle = T1.angdist(T2)
            assert abs(test_angle - angle) < 1e-6

[myeatman-bdai]

Hey Peter, just viewing your comment now, I definitely agree that performing normalization by default in all these cases would be a good improvement.

I probably won't be able to put up a PR or review one until next week.