0
$\begingroup$

I have a time series of camera locations $L_i$ = $\langle Q_i, P_i \rangle$, with each camera location having an initially unknown global orientation quaternion $Q_i$ and global position vector $P_i$. Between each pair of adjacent camera locations $L_i$ and $L_{i+1}$, there is a known mapping $\langle R_i, T_i \rangle$ consisting of a local rotation quaternion $R_i$ and a local translation vector $T_i$ that rotate and then translate the camera from $L_{i+1}$ back to the local coordinate system of $L_i$ (i.e. $L_i$ is fixed, and the local transformations map or register $L_{i+1}$ to $L_i$). Given a point $X_{i+1}$ representing the coordinates of a specific point $X$ in the local coordinate system of $L_{i+1}$, the coordinates of $X$ may be found for the local coordinate system of $L_i$ using:

$X_i = R_i X_{i+1} R_i^{-1} + T_i$

and therefore,

$X_{i-1} = R_{i-1} (R_i X_{i+1} R_i^{-1} + T_i) R_{i-1}^{-1} + T_{i-1}$.

This rule may be applied iteratively to eventually find $X_0$, which is the coordinates of $X_{i+1}$ in the global coordinate system.

However, applying these transformations backwards, from the last camera location to the first, for a series of 3D point clouds $X_i^j$ captured from each camera location $L_i$, is an $O(N^2)$ operation in the number of camera locations $N$, since all points in each point cloud for a camera location $L_i$ have to be transformed by all $\langle R_j, T_j \rangle$ for $0 \le j \lt i$. Therefore, I would like to invert these transformations so that the "inverse" of each local transform can simply be concatenated to produce a new set of transformations $\langle Q_i, P_i \rangle$ that directly transform a point $X_i$ to $X_0$ (i.e. that directly transform from the coordinate system of $L_i$ into the coordinate system of $L_0$) via the single transform:

$X_0 = Q_i X_i Q_i^{-1} + P_i$

The core question: How can $Q_{i}$ and $P_{i}$ be computed from $\langle R_j, T_j \rangle$, for $0 \le j \lt i\,$? (Note that it is fine to assume that $L_0$ is already in the global coordinate system, rather than $L_{N-1}$, when working from the beginning of the list of transformations, i.e. $Q_0$ is the identity and $P_0$ is zero.)

The problem is that the transformations are defined as a chain back from the last camera position to the first, but I need the cumulative concatenation of transforms from the first camera position to the last. I think this requires inverting the relative order of rotation and translation: given $A_i X_i A_i^{-1} + B_i$, how do you find the equivalent transformation expressed as $C_i (X_i + D_i) C_i^{-1}$?

$\endgroup$
5
  • $\begingroup$ To simplify my answer: use $P_{i+1} = P_i + Q_i^\prime T_i Q_i$, because you want to invert the rotation $Q_i$. $\endgroup$
    – None
    Jul 26, 2020 at 13:33
  • $\begingroup$ Thanks, I tried your solution and it's still not working either for some reason, but I'm sure I have something wrong with my implementaiton. $\endgroup$ Jul 27, 2020 at 4:58
  • 1
    $\begingroup$ The inverse of transformation$$X_0 = Q_i X_i Q_i^{-1} + P_i$$is$$X_i = Q_i^{-1} ( X_0 - P_i ) Q_i = Q_i^{-1} X_0 Q_i - Q_i^{-1} P_i Q_i$$where $Q_i$ is an unit quaternion and $X_0$, $X_i$, and $P_i$ are vectors. If $$L_i = R_i L_{i+1} R_i^{-1} + T_i$$then$$L_{i+1} = R_i^{-1} (L_i - T_i) R_i = R_i^{-1} L_i R_i - R_i^{-1} T_i R_i$$where $L_i$, $L_{i+1}$, and $T_i$ are vectors, and $R_i$ is an unit quaternion. $\endgroup$
    – None
    Jul 28, 2020 at 0:07
  • 1
    $\begingroup$ In your question, the expression for $X_{i-1}$ is missing a $R_{i-1}^{i-1}$ just after the closing parenthesis. $\endgroup$
    – None
    Jul 28, 2020 at 0:47
  • $\begingroup$ Thanks, I somehow missed the idea that quaternion multiplication distributes over vector addition. That's obvious in hindsight! $\endgroup$ Jul 28, 2020 at 1:22

2 Answers 2

1
$\begingroup$

Third take. Stop changing the definitions! :)

We have a sequence of cumulative rotation unit quaternions $R_i$ and cumulative translations $T_i$ in each local coordinate system, with $Q_i$ describing the rotation from the global coordinate system to local coordinate system at step $i$, and $P_i$ being the sum total translations up to and including step $i$ in the global coordinate system:

$$Q_i = Q_{i-1} R_{i-1} \tag{1a}\label{None1a}$$ or equivalently $$Q_i = Q_{i+1} R_i^{-1} \tag{1b}\label{None1b}$$ and $$P_i = P_{i-1} + Q_{i-1} T_{i-1} Q_{i-1}^{-1} \tag{2a}\label{None2a}$$ or equivalently $$P_i = P_{i+1} - Q_i T_i Q_i^{-1} \tag{2b}\label{None2b}$$

We can use $\eqref{None1a}$ or $\eqref{None1b}$ to construct $Q_0^\prime, Q_1^\prime, \dots, Q_{N-1}^\prime, Q_{N}^\prime$. With $\eqref{None1a}$, set $Q_N^\prime = 1$ and iterate $i = N-1, N-2, \dots, 2, 1, 0$. With $\eqref{None1b}$, set $Q_0\prime = 0$ and iterate $i = 1, 2, \dots, N-1, N$.

Then, fix $Q_k = 1$ (where $0 \le k \le N$), by iterating $$Q_i = Q_k^{\prime -1} Q_i^\prime, \quad \forall i \tag{3}\label{None3}$$

After you have the orientations, you can calculate the total translations $P_i$ using $$P_0^\prime = \vec{0}, \quad P_i^\prime = P_{i-1}^\prime + Q_{i-1} T_{i-1} Q_{i-1}^{-1} \tag{4a}\label{None4a}$$ or $$P_N^\prime = \vec{0}, \quad P_i^\prime = P_{i+1}^\prime - Q_i T_i Q_i^{-1} \tag{4b}\label{None4b}$$ which will yield the same translations except for a constant difference. Again, to fix $P_k = \vec{0}$ for some $0 \le k \le N$, iterate $$P_i = P_i^\prime - P_k^\prime, \quad \forall i \tag{5}\label{None5}$$

As you can clearly see, this has $O(N)$ time complexity.

If $k = N$, use the $(a)$ methods; and if $k = 0$, the $(b)$ methods. This way the fixup is an identity operation, and can be skipped.

Note that with the fixup pass, both $(a)$ and $(b)$ work for all $k$, including $k = 0$ and $k = N$. Choosing one over the other is just an optimization.


Here is an example Python3 program, that implements two helper classes, Vector and Versor, and implements the above logic. (If you save this as example.py, you can use pydoc3 example to see the API it provides. It is in Public Domain, and you can use the classes in your own scripts if you put it in the same directory, and add from example import Vector, Versor.)

"""3D Euclidean vector class 'Vector', and unit quaternion class 'Versor'
   -- a playground for experimenting with rotations in 3D"""

# SPDX-License-Identifier: CC0-1.0

from math import sqrt, pi, sin, cos, atan2

class Vector(tuple):
    """3D Cartesian vector type"""

    FORMAT = "(%8.5f,%8.5f,%8.5f)"

    def __new__(cls, x, y, z):
        """Create a new vector"""
        return tuple.__new__(cls, (float(x), float(y), float(z)))

    @property
    def x(self):
        """x coordinate"""
        return self[0]

    @property
    def y(self):
        """y coordinate"""
        return self[1]

    @property
    def z(self):
        """z coordinate"""
        return self[2]

    @property
    def norm(self):
        """Euclidean length"""
        return sqrt(self[0]*self[0] + self[1]*self[1] + self[2]*self[2])

    @property
    def normsqr(self):
        """Euclidean length squared"""
        return self[0]*self[0] + self[1]*self[1] + self[2]*self[2]

    @property
    def unit_vector(self):
        """Scaled to unit length"""
        n = sqrt(self[0]*self[0] + self[1]*self[1] + self[2]*self[2])
        if n > 0:
            return tuple.__new__(Vector, (self[0]/n, self[1]/n, self[2]/n))
        else:
            return tuple.__new__(Vector, (0, 0, 0))

    def perpendicular_to(self, other):
        """Part perpendicular to another vector"""
        nn = float(other[0])**2 + float(other[1])**2 + float(other[2])**2
        if nn > 0:
            d = (self[0]*other[0] + self[1]*other[1] + self[2]*other[2]) / nn
            return tuple.__new__(Vector, (self[0] - d*other[0],
                                          self[1] - d*other[1],
                                          self[2] - d*other[2]))
        else:
            return tuple.__new__(Vector, (0, 0, 0))

    def transform(self, matrix, translate=(0,0,0), pretranslate=(0,0,0)):
        """Transform this point by rotation matrix and translation"""
        curr = Vector(self[0]+pretranslate[0], self[1]+pretranslate[1], self[2]+pretranslate[2])
        post = Vector(translate)
        return Vector(matrix[0][0]*curr[0] + matrix[0][1]*curr[1] + matrix[0][2]*curr[2] + post[0],
                      matrix[1][0]*curr[0] + matrix[1][1]*curr[1] + matrix[1][2]*curr[2] + post[1],
                      matrix[2][0]*curr[0] + matrix[2][1]*curr[1] + matrix[2][2]*curr[2] + post[2])

    def __str__(self):
        """Conversion to string"""
        return Vector.FORMAT % self

    def __abs__(self):
        """Absolute value is the Euclidean length"""
        return sqrt(self[0]*self[0] + self[1]*self[1] + self[2]*self[2])

    def __neg__(self):
        """Opposite vector, negation"""
        return tuple.__new__(Vector, (-self[0], -self[1], -self[2]))

    def __bool__(self):
        """Nonzero vectors are considered True, zero vectors False"""
        return (self[0]*self[0] + self[1]*self[1] + self[2]*self[2] > 0)

    def __add__(self, other):
        """Vector addition"""
        return tuple.__new__(Vector, (self[0]+other[0], self[1]+other[1], self[2]+other[2]))

    def __radd__(self, other):
        """Vector addition"""
        return tuple.__new__(Vector, (other[0]+self[0], other[1]+self[1], other[2]+self[2]))

    def __sub__(self, other):
        """Vector subtraction"""
        return tuple.__new__(Vector, (self[0]-other[0], self[1]-other[1], self[2]-other[2]))
        
    def __radd__(self, other):
        """Vector subtraction"""
        return tuple.__new__(Vector, (other[0]-self[0], other[1]-self[1], other[2]-self[2]))

    def __mul__(self, scalar):
        if isinstance(scalar, (int, float)):
            return tuple.__new__(Vector, (self[0]*scalar, self[1]*scalar, self[2]*scalar))
        else:
            return NotImplemented

    def __rmul__(self, scalar):
        if isinstance(scalar, (int, float)):
            return tuple.__new__(Vector, (scalar*self[0], scalar*self[1], scalar*self[2]))
        else:
            return NotImplemented

    def __truediv__(self, scalar):
        if isinstance(scalar, (int, float)):
            return tuple.__new__(Vector, (self[0]/scalar, self[1]/scalar, self[2]/scalar))
        else:
            return NotImplemented

    def __rtruediv__(self, ignored):
        return NotImplemented

    def __or__(self, other):
        """Dot product, a | b"""    
        if isinstance(other, (list, tuple)) and len(other) == 3:
            return self[0]*other[0] + self[1]*other[1] + self[2]*other[2]
        else:
            return NotImplemented

    def dot(self, other):
        """Dot product"""
        return self[0]*other[0] + self[1]*other[1] + self[2]*other[2]

    def __xor__(self, other):
        """Cross product, a ^ b"""
        if isinstance(other, (list, tuple)) and len(other) == 3:
            return tuple.__new__(Vector, ( self[1]*other[2] - self[2]*other[1],
                                           self[2]*other[0] - self[0]*other[2],
                                           self[0]*other[1] - self[1]*other[0] ))
        else:
            return NotImplemented

    def cross(self, other):
        """Cross product"""
        return tuple.__new__(Vector, ( self[1]*other[2] - self[2]*other[1],
                                       self[2]*other[0] - self[0]*other[2],
                                       self[0]*other[1] - self[1]*other[0] ))


class Versor(tuple):
    """Unit quaternion type describing an orientation or a rotation"""

    FORMAT = "(%8.5f;%8.5f,%8.5f,%8.5f)"

    def __new__(cls, w, x, y, z):
        """Create a new versor"""
        w = float(w)
        x = float(x)
        y = float(y)
        z = float(z)
        n = sqrt(w*w + x*x + y*y + z*z)
        if n == 0:
            w = 1
            x = 0
            y = 0
            z = 0
        elif n != 1:
            w /= n
            x /= n
            y /= n
            z /= n
        return tuple.__new__(cls, (w, x, y, z))

    @classmethod
    def from_axis_angle(cls, axis, angle):
        """Create a versor from an axis and angle in degrees"""
        x = float(axis[0])
        y = float(axis[1])
        z = float(axis[2])
        h = angle * pi / 360.0
        n = sqrt(x*x + y*y + z*z)
        if n == 0:
            return tuple.__new__(cls, (1, 0, 0, 0))
        s, c = sin(h), cos(h)
        return tuple.__new__(cls, (c, s*x/n, s*y/n, s*z/n))

    def __str__(self):
        """Conversion to string"""
        return Versor.FORMAT % self

    def __abs__(self):
        return sqrt(self[0]*self[0] + self[1]*self[1] + self[2]*self[2])

    def __neg__(self):
        return Versor(-self[0], -self[1], -self[2], -self[3])

    def __mul__(self, other):
        if isinstance(other, (int, float)):
            return (self[0]*other, self[1]*other, self[2]*other, self[3]*other)            
        elif isinstance(other, (list, tuple)) and len(other) == 4:
            return (self[0]*other[0] - self[1]*other[1] - self[2]*other[2] - self[3]*other[3],
                    self[0]*other[1] + self[1]*other[0] + self[2]*other[3] - self[3]*other[2],
                    self[0]*other[2] - self[1]*other[3] + self[2]*other[0] + self[3]*other[1],
                    self[0]*other[3] + self[1]*other[2] - self[2]*other[1] + self[3]*other[0])
        else:
            return NotImplemented

    def __rmul__(self, other):
        if isinstance(other, (int, float)):
            return (other*self[0], other*self[1], other*self[2], other*self[3])
        elif isinstance(other, (list, tuple)) and len(other) == 4:
            return (other[0]*self[0] - other[1]*self[1] - other[2]*self[2] - other[3]*self[3],
                    other[0]*self[1] + other[1]*self[0] + other[2]*self[3] - other[3]*self[2],
                    other[0]*self[2] - other[1]*self[3] + other[2]*self[0] + other[3]*self[1],
                    other[0]*self[3] + other[1]*self[2] - other[2]*self[1] + other[3]*self[0])
        else:
            return NotImplemented

    @property
    def w(self):
        """Versor real component"""
        return self[0]

    @property
    def x(self):
        """Versor vector x component"""
        return self[1]

    @property
    def y(self):
        """Versor vector y component"""
        return self[2]

    @property
    def z(self):
        """Versor vector z component"""
        return self[3]

    @property
    def axis(self):
        """Rotation unit axis vector"""
        n = sqrt(self[1]*self[1] + self[2]*self[2] + self[3]*self[3])
        if n > 0:
            return Vector(self[1]/n, self[2]/n, self[3]/n)
        else:
            return Vector(0, 0, 0)

    @property
    def angle(self):
        """Rotation angle, in degrees"""
        n = sqrt(self[1]*self[1] + self[2]*self[2] + self[3]*self[3])
        if n > 0:
            return atan2(n, self[0]) * 360.0 / pi

    @property
    def normalized(self):
        """Versor normalized to unit length"""
        n = sqrt(self[0]*self[0] + self[1]*self[1] + self[2]*self[2] + self[3]*self[3])
        if n == 0:
            return tuple.__new__(Versor, (1, 0, 0, 0))
        elif n != 1:
            return tuple.__new__(Versor, (self[0]/n, self[1]/n, self[2]/n, self[3]/n))
        else:
            return self

    @property
    def inverse(self):
        """Inverse of this versor"""
        return tuple.__new__(Versor, (self[0], -self[1], -self[2], -self[3]))

    @property
    def matrix(self):
        """Rotation matrix corresponding to this versor"""
        c01 = 2*self[0]*self[1]
        c02 = 2*self[0]*self[2]
        c03 = 2*self[0]*self[3]
        c11 = 2*self[1]*self[1]
        c12 = 2*self[1]*self[2]
        c13 = 2*self[1]*self[3]
        c22 = 2*self[2]*self[2]
        c23 = 2*self[2]*self[3]
        c33 = 2*self[3]*self[3]
        return ( Vector(1-c22-c33, c12-c03, c13+c02),
                 Vector(c12+c03, 1-c11-c33, c23-c01),
                 Vector(c13-c02, c23+c01, 1-c11-c22) )

    def rotate(self, other):
        """For vector v, q.rotate(v) calculates q v q^-1 .
           For quaternion p, q.rotate(p) calculates q p."""
        if isinstance(other, (list, tuple)) and len(other) == 3:
            x = float(other[0])
            y = float(other[1])
            z = float(other[2])
            c00 = self[0]*self[0]
            c11 = self[1]*self[1]
            c22 = self[2]*self[2]
            c33 = self[3]*self[3]
            c01 = 2*self[0]*self[1]
            c02 = 2*self[0]*self[2]
            c03 = 2*self[0]*self[3]
            c12 = 2*self[1]*self[2]
            c13 = 2*self[1]*self[3]
            c23 = 2*self[2]*self[3]
            return Vector(x*(c00+c11-c22-c33) + y*(c12-c03) + z*(c02+c13),
                          x*(c03+c12) + y*(c00-c11+c22-c33) + z*(c23-c01),
                          x*(c13-c02) + y*(c01+c23) + z*(c00-c11-c22+c33))
        elif isinstance(other, (list, tuple)) and len(other) == 4:
            return Versor(self[0]*other[0] - self[1]*other[1] - self[2]*other[2] - self[3]*other[3],
                          self[0]*other[1] + self[1]*other[0] + self[2]*other[3] - self[3]*other[2],
                          self[0]*other[2] - self[1]*other[3] + self[2]*other[0] + self[3]*other[1],
                          self[0]*other[3] + self[1]*other[2] - self[2]*other[1] + self[3]*other[0])
        else:
            raise ValueError("Cannot rotate a %s" % str(type(other)))

    def rotated_by(self, other):
        """For quaternion p, q.rotated_by(p) calculates p q."""
        w = other[0]*self[0] - other[1]*self[1] - other[2]*self[2] - other[3]*self[3]
        x = other[0]*self[1] + other[1]*self[0] + other[2]*self[3] - other[3]*self[2]
        y = other[0]*self[2] - other[1]*self[3] + other[2]*self[0] + other[3]*self[1]
        z = other[0]*self[3] + other[1]*self[2] - other[2]*self[1] + other[3]*self[0]
        n = sqrt(w*w + x*x + y*y + z*z)
        if n == 0:
            return tuple.__new__(Versor, (1, 0, 0, 0))
        elif n != 1:
            w /= n
            x /= n
            y /= n
            z /= n
        return tuple.__new__(Versor, (w, x, y, z))

    def transform(self, points, translate=(0, 0, 0), pretranslate=(0,0,0)):
        result = []
        pre = Vector(pretranslate[0], pretranslate[1], pretranslate[2])
        post = Vector(translate[0], translate[1], translate[2])
        for p in points:
            result.append(self.rotate(Vector(p) + pre) + post)
        return result

if __name__ == '__main__':
    from random import Random
    rng = Random()

    N = 10

    # Cumulative inverse rotations and local translations
    R = []
    T = []
    for i in range(0, N):
        while True:
            x = rng.uniform(-1,+1)
            y = rng.uniform(-1,+1)
            z = rng.uniform(-1,+1)
            n = sqrt(x*x + y*y + z*z)
            if n > 0.1 and n < 1.0:
                break
        R.append(Versor.from_axis_angle(Vector(x, y, z), rng.uniform(-180, 180)))
        T.append(Vector(rng.uniform(-2,+2), rng.uniform(-2,+2), rng.uniform(-2,+2)))

    n = N - 1

    # Valid range for k is 0..N, inclusive.
    k = round(rng.uniform(-0.49, N+0.49))

    Qa = [ None ]*(N+1)
    Qb = [ None ]*(N+1)
    Pa = [ None ]*(N+1)
    Pb = [ None ]*(N+1)

    #
    # Orientation
    #

    # a) Forwards
    Qa[0] = Versor(1,0,0,0)
    for i in range(1, N+1):     # i = 1, 2, ..., N-1, N.
        Qa[i] = Qa[i-1].rotate(R[i-1])

    # b) Backwards
    Qb[N] = Versor(1,0,0,0)
    for i in range(N-1,-1,-1):  # i = N-1, N-2, ..., 1, 0.
        Qb[i] = Qb[i+1].rotate(R[i].inverse)
      
    print("Orientation quaternions before fixup pass:")
    for i in range(0, N+1):     # i = 0, 1, ..., N-1, N.
        print("%s : %s" % (str(Qa[i]), str(Qb[i])))

    # Orientation fixup.
    QaFix = Qa[k].inverse
    QbFix = Qb[k].inverse
    for i in range(0, N+1):     # i = 0, 1, ..., N-1, N.
        Qa[i] = QaFix.rotate(Qa[i])
        Qb[i] = QbFix.rotate(Qb[i])

    #
    # Position
    #

    # a) Forwards
    Pa[0] = Vector(0,0,0)
    for i in range(1, N+1):     # i = 1, 2, ..., N-1, N.
        Pa[i] = Pa[i-1] + Qa[i-1].rotate(T[i-1])

    # b) Backwards
    Pb[N] = Vector(0,0,0)
    for i in range(N-1,-1,-1):  # i = N-1, N-2, ..., 1, 0.
        Pb[i] = Pb[i+1] - Qb[i].rotate(T[i])

    # Position fixup.
    PaFix = Pa[k]
    PbFix = Pb[k]
    for i in range(0, N+1):     # i = 0, 1, ..., N-1, N.
        Pa[i] = Pa[i] - PaFix
        Pb[i] = Pb[i] - PbFix

    print("Orientation quaternions after fixup pass for k=%d:" % k)
    qerr = [ 0, 0, 0, 0 ]
    perr = [ 0, 0, 0, 0 ]
    for i in range(0, N+1):     # i = 0, 1, ..., N-1, N.
        print("%s = %s   %s = %s" % (str(Qa[i]), str(Qb[i]), str(Pa[i]), str(Pb[i])))
        qerr = [ max(qerr[0], abs(Qa[i][0] - Qb[i][0])),
                 max(qerr[1], abs(Qa[i][1] - Qb[i][1])),
                 max(qerr[2], abs(Qa[i][2] - Qb[i][2])),
                 max(qerr[3], abs(Qa[i][3] - Qb[i][3])) ]
        perr = [ max(perr[0], abs(Pa[0] - Pb[0])),
                 max(perr[1], abs(Pa[1] - Pb[1])),
                 max(perr[2], abs(Pa[2] - Pb[2])) ]

    print("Max errors: %.6f %.6f %.6f %.6f   %.6f %.6f %.6f" % (*qerr, *perr))

When run, the program generates ten rotations and translations, and uses the two iteration directions to solve $Q_i$ and $P_i$ with a random $k$.

The output contains first $Q_a^\prime$ and $Q_b^\prime$ before the fixup pass, then $Q_a$, $Q_b$, $P_a$, and $P_b$. If this approach works, then $Q_a = Q_b$ and $P_a = P_b$, with a selected row ($k = 0$ being the first) having $Q_a = Q_b = 1$ and $P_a = P_b = \vec{0}$. The last line reports the maximum component-wise differences between each pair of $Q_a$, $Q_b$ and $P_a$, $P_b$.

Here is the output from an example run:

Orientation quaternions before fixup pass:
( 1.00000; 0.00000, 0.00000, 0.00000) : ( 0.77039;-0.19139,-0.58682,-0.15973)
( 0.41966;-0.19167,-0.83338, 0.30435) : (-0.15381;-0.53969,-0.79943, 0.21446)
( 0.69413; 0.48687,-0.52190, 0.09363) : ( 0.33662; 0.10393,-0.86924, 0.34685)
( 0.77771; 0.49771,-0.37579,-0.07897) : ( 0.46126; 0.22090,-0.84049, 0.17893)
( 0.92556; 0.28991,-0.23517,-0.06307) : ( 0.62045; 0.04565,-0.78269, 0.01871)
(-0.24442; 0.33496,-0.88851,-0.19650) : (-0.67698; 0.27822,-0.63217, 0.25427)
( 0.40926; 0.48004,-0.01805,-0.77572) : ( 0.27267; 0.74382,-0.47920,-0.37783)
(-0.34109; 0.24307,-0.89218,-0.16908) : (-0.76681; 0.20925,-0.55835, 0.23762)
(-0.48096; 0.51707, 0.30692,-0.63805) : (-0.19338; 0.91384, 0.31398,-0.17004)
( 0.45215; 0.51974, 0.68178, 0.24620) : ( 0.88721; 0.27829, 0.22401, 0.29195)
( 0.77039; 0.19139, 0.58682, 0.15973) : ( 1.00000; 0.00000, 0.00000, 0.00000)
Orientation quaternions after fixup pass for k=5:
(-0.24442;-0.33496, 0.88851, 0.19650) = (-0.24442;-0.33496, 0.88851, 0.19650)   (-0.36075, 0.15348,-3.62981) = (-0.36075, 0.15348,-3.62981)
( 0.51388; 0.34046, 0.64085, 0.45752) = ( 0.51388; 0.34046, 0.64085, 0.45752)   ( 0.47612,-0.59985,-2.00515) = ( 0.47612,-0.59985,-2.00515)
( 0.43873;-0.16576, 0.87133,-0.14426) = ( 0.43873;-0.16576, 0.87133,-0.14426)   ( 0.18914,-0.35767,-1.86871) = ( 0.18914,-0.35767,-1.86871)
( 0.32603;-0.37848, 0.85420,-0.14422) = ( 0.32603;-0.37848, 0.85420,-0.14422)   ( 1.60830,-2.51529,-2.32274) = ( 1.60830,-2.51529,-2.32274)
( 0.09222;-0.39072, 0.91569, 0.01848) = ( 0.09222;-0.39072, 0.91569, 0.01848)   ( 0.61687,-1.03912,-1.88966) = ( 0.61687,-1.03912,-1.88966)
( 1.00000; 0.00000, 0.00000,-0.00000) = ( 1.00000; 0.00000,-0.00000, 0.00000)   ( 0.00000, 0.00000, 0.00000) = ( 0.00000, 0.00000, 0.00000)
( 0.22923;-0.94011, 0.20253,-0.15045) = ( 0.22923;-0.94011, 0.20253,-0.15045)   ( 1.83789,-0.53006,-0.60693) = ( 1.83789,-0.53006,-0.60693)
( 0.99072; 0.07993,-0.09386, 0.05718) = ( 0.99072; 0.07993,-0.09386, 0.05718)   ( 2.73874,-2.16200,-1.44103) = ( 2.73874,-2.16200,-1.44103)
( 0.14344;-0.59250,-0.61448,-0.50078) = ( 0.14344;-0.59250,-0.61448,-0.50078)   ( 2.76191,-1.35384,-0.52793) = ( 2.76191,-1.35384,-0.52793)
(-0.59057;-0.19371, 0.41969,-0.66149) = (-0.59057;-0.19371, 0.41969,-0.66149)   ( 4.10824,-0.82590,-2.47648) = ( 4.10824,-0.82590,-2.47648)
(-0.67698;-0.27822, 0.63217,-0.25427) = (-0.67698;-0.27822, 0.63217,-0.25427)   ( 5.38527,-3.28029,-2.42548) = ( 5.38527,-3.28029,-2.42548)
Max errors: 0.000000 0.000000 0.000000 0.000000   0.000000 0.000000 0.000000

Hopefully this suffices as a numerical proof that this approach works.

$\endgroup$
5
  • $\begingroup$ Got it! Here is the solution code. pastebin.com/54Ticb2E $\endgroup$ Jul 28, 2020 at 6:39
  • $\begingroup$ My previous update was wrong again. These are the correct update rules: $$Q_i = Q_{i-1} R_{i-1}$$ $$X_i = X_{i-1} + Q_{i-1} T_{i-1} Q_{i-1}^{-1}$$ $\endgroup$ Jul 28, 2020 at 13:07
  • $\begingroup$ Thanks for updating your post -- your solution matches mine now. I appreciate your patience, and the effort you put into answering this question! $\endgroup$ Jul 29, 2020 at 1:11
  • $\begingroup$ Sorry, I wasn't trying to change the definitions -- I just had to go through an iterative process of phrasing mathematically exactly what I was trying to compute. $\endgroup$ Jul 29, 2020 at 2:55
  • 1
    $\begingroup$ @LukeHutchison: No worries, that's what I did here too. It happens! :) $\endgroup$
    – None
    Jul 29, 2020 at 10:59
0
$\begingroup$

The answer by "None" is the same as the answer I arrived at. I'm posting my working here:

To map $X_i$, the local coordinates of a point $X$ captured in the coordinate system of camera position $L_i$, to $X_{i-1}$, the coordinates of the point in the coordinate system of camera position $L_{i-1}$, using the known relative rotation quaternion $R_{i-1}$ and translation vector $T_{i-1}$, we can use the following equation (this is obtained directly from the definition of how these relative rotations and translations are defined):

$$X_{i-1} = R_{i-1} X_i R_{i-1}^{-1} + T_{i-1}\tag{1}$$

So to map $X_i$ (in the coordinate system of $L_i$) to $X_{i-2}$ (in the coordinate system of $L_{i-2}$), we obtain:

$$X_{i-2} = R_{i-2} (R_{i-1} X_i R_{i-1}^{-1} + T_{i-1}) R_{i-2}^{-1} + T_{i-2}$$

$$= (R_{i-2} R_{i-1} X_i R_{i-1}^{-1} R_{i-2}^{-1}) + (R_{i-2} T_{i-1} R_{i-2}^{-1} + T_{i-2})$$

where the lefthand operand of the toplevel addition is the "rotation part" and the righthand operand is the "translation part".

Similarly, to map $X_i$ (in the coordinate system of $L_i$) to $X_{i-3}$ (in the coordinate system of $L_{i-3}$), we obtain:

$$X_{i-3} = R_{i-3} ((R_{i-2} R_{i-1} X_i R_{i-1}^{-1} R_{i-2}^{-1}) + (R_{i-2} T_{i-1} R_{i-2}^{-1} + T_{i-2})) R_{i-3}^{-1} +T_{i-3}$$

$$= R_{i-3} (R_{i-2} R_{i-1} X_i R_{i-1}^{-1} R_{i-2}^{-1}) R_{i-3}^{-1} + R_{i-3} (R_{i-2} T_{i-1} R_{i-2}^{-1} + T_{i-2}) R_{i-3}^{-1} +T_{i-3}$$

$$ = (R_{i-3} R_{i-2} R_{i-1} X_i R_{i-1}^{-1} R_{i-2}^{-1} R_{i-3}^{-1}) + (R_{i-3} R_{i-2} T_{i-1} R_{i-2}^{-1} R_{i-3}^{-1} + (R_{i-3} T_{i-2} R_{i-3}^{-1} +T_{i-3}))$$

We want to compute $X_0$, the global coordinates of $X_i$, i.e. the coordinates of $X_i$ in the coordinate system of camera location $L_0$. Here $Q_i$ and $P_i$ represent the orientation quaternion and translation vector respectively for the camera position $L_i$ in the coordinate system of $L_0$:

$$X_0 = Q_i X_i Q_i^{-1} + P_i\tag{2}$$

This yields the following orientation and position update rules:

$$Q_i = Q_{i-1}R_{i-1} \, \, \, ; \, \, \, Q_0 = 1\tag{3}$$ $$P_i = Q_{i-1} T_{i-1} Q_{i-1}^{-1} + P_{i-1} \, \, \, ; \, \, \,P_0 = [0 \,\, 0 \,\, 0]\tag{4}$$

So we started from a transformation mapping $X_i$ to $X_{i-1}$ (Eq. 1), and ended up with a pair of transformations mapping $Q_{i-1}$ to $Q_{i}$ and $P_{i-1}$ to $P_i$ (Eqs. 2, 3), i.e. $P_i$ and $Q_i$ can be computed incrementally, in a sequence starting from $\langle P_0, Q_0 \rangle$ and ending with $\langle P_i, Q_i \rangle$. The transformations $Q_i$ and $P_i$ can then be used to map $X_i$ back to $X_0$ (Eq. 2).

$\endgroup$

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .