1
$\begingroup$

Given:

  1. a sequence ${A_i}_{i=1}^n$ of elements of the special Euclidean group, $SE(3)$;
  2. a sequence of times ${t_i}_{i=1}^n$, where $t_1 \leq t_2 \leq ... \leq t_n$;
  3. instantaneous body frame velocity given by a twist $v\in se(3)$ at time $t_1$
  4. instantaneous body frame acceleration $\alpha$ at time $t_1$

I would like to find an interpolating curve $f(t) \in SE(3)$ that is twice differentiable such that $f(t_i) = A_i$ and $f'(t_1) = v$ and $f''(t_1) = \alpha$.

One approach, as suggested by [1], is:

  1. Compute $\xi_1, ..., \xi_n \in se(3)$ where $\xi_i = \log(A_1^{-1} A_i)$.
  2. Compute an interpolating curve (e.g. using a B-spline) $\gamma(t)$ in Euclidean space $\mathbb R^6$ such that $\gamma(t_i) = \xi_i$.

Then, the interpolating curve is $f(t) = A_1 \exp(\gamma(t))$.

Unfortunately, this doesn't work well in most places except for a small neighbourhood around $A_1$, because $\exp$ i s a surjective map from $se(3)$ to $SE(3)$. For example, if we interpret the angular part of $\xi$ as an axis-angle representation in $so(3)$, you can flip the sign on the axis and replace the angle $\theta$ with $2\pi - \theta$ and it would represent the same element in $SO(3)$. As a consequence, if some elements $A_i$, $A_{i+1}$ happen to be close together in $SE(3)$ but cross over the $\pi$ threshold, $\xi_i$ and $\xi_{i+1}$ may not be close together in $\mathbb R^6$ and the interpolating curve may take an additional rotation than what would be "natural".

An alternative is to interpolate separately in rotation and translation. However, most rotation formalisms fail in some case. For example, Gibbs vectors (used by [2]) suffer from the drawback that $180^\circ$ rotations cannot be represented. Quaternions (used by [3]) also have a sign ambiguity, so two elements close together in $SO(3)$ are also not guaranteed to be close in quaternion space. Also, an interpolating function in $\mathbb R^4$ may not be normalized in general, and may become singular if it passes through zero (though it is unlikely). I have no idea how the authors in the cited papers overcome these issues.

Besides, using the exponential map is most natural when dealing with nonlinear optimization on the manifold, since the derivatives are easy to compute.

Some help and insights for tackling this problem or working around the drawbacks would be appreciated.

[1] Žefran, M., & Kumar, V. (1998). Interpolation schemes for rigid body motions. Computer-Aided Design, 30(3), 179-189

[2] Alismail, H., Baker, L. D., & Browning, B. (2014, May). Continuous trajectory estimation for 3D SLAM from actuated lidar. In 2014 IEEE International Conference on Robotics and Automation (ICRA) (pp. 6096-6101). IEEE.

[3] Zlot, R., & Bosse, M. (2014). Efficient Large‐scale Three‐dimensional Mobile Mapping for Underground Mines. Journal of Field Robotics, 31(5), 758-779.

1 Answers 1

2

James Kajiya wrote a paper back in the 1990s, I think, on spline-like interpolation on manifolds, and that might be worth looking at.

But the really rotten thing is that it's just darned tough to know what the answer should be. Let's forget translation for the moment, and let's simplify to knowing only initial and final value and derivative (i.e., make the problem first-order). And let's assume that you want something really spline-like: it's smooth, it's almost "polynomial", it has all the nice symmetry properties one might hope for, like "if I apply a transformation $T$ to all the input data, then the resulting "spline" should just be $T$ applied to the spline for the untransformed data". When $T$ is something like a reflection in a plane, this places symmetry constraints on your interpolation scheme.

OK, back to my example:

Make the initial orientation be the identity, with a derivative saying "start rotating $x$ towards $y$, leaving $z$ fixed." Let the final orientation be $P_s$, which is rotation by angle $s$ about the $z$-axis, and the final derivative being the same as the initial one.

Now any "spline like" interpolant is going to be continuous as a function of those input parameters. So as the final orientation $s$ is changed from, say, $\pi/4$ to $\pi/2$, the resulting path will change from "rotate a little around $z$" to "rotate a bit more around $z$", and so on. What happens when $s$ grows almost to $2\pi$? It doesn't really matter much to me what your answer is, but then ask yourself "and what will happen when $s$ gets a little larger than $2\pi$, say, $2\pi + \pi/4$? As an element of SO(3), that's indistinguishable from $s = \pi/4$. Are your interpolating curves the same for these two?

By symmetry, your interpolating curves must all be in the $SO(2)$ subgroup of $SO(3)$ corresponding to rotations about $z$.

I'm hoping that at this point you see the problem you're in: if you assume symmetry, and continuity as a function of input data, you end up with a fundamental-group problem: one path from the identity to $\pi/4$ , followed by the inverse of the other, represents a nontrivial element of the fundamental group. But if you take the one path and follow it by the reverse of itself, you get a trivial element of $\pi_1$. Consequence: things can't actually be continuous the way you wanted.

The authors of the cited papers probably don't overcome those problems, because they cannot be overcome. They may choose to weaken their hypotheses (to hell with continuity!) or something else. But you shouldn't frown on them for failing to solve an insoluble problem. :)