# Estimate for a rigid transform given a set of noisy measurements

I have a set of rigid transforms $\in \mathbb{R}^{4x4}$, where each transform is an approximation to some unknown, "correct" transform. I'm looking for an algorithm to estimate the correct transform given the approximations. My first thought would be to take the element-wise average over the entries in the matrices, and then re-normalize the resulting matrix to get a rigid transform, but I bet there's a better way to do it. Does anyone have any advice?

Thanks!

-

Background

First, let us try to understand the problem better. What does a set of approximations $x_1,...,x_n$ of an unknown correct entity $\hat{x}$ mean? Well, this can be understood as a set of samples from a Gaussian distribution:

$$x_1,...,x_n \quad\tilde{}\quad {\cal N}(\hat{x},\Sigma).$$

Here, $\Sigma$ is the sample covariance, thus the assumed uncertainty of the approximations. In other words, $x_i = \hat{x}+\epsilon$ with $\epsilon$ being some noise term drawn from the zero-mean Gaussian ${\cal N}(0,\Sigma)$.

The problem is the underlying assumption here: $\hat{x}$ is a vector, thus element of a vector space. Otherwise, it would not make sense to talk about covariance matrices.

However, an element $T =[R,t]$ of the group of rigid transformation is not a vector (e.g, $s\cdot[R,t] = [sR,st]$ is not a valid rigid transformation since $sR$ is not a valid rotation for $s\neq1$). So, how to sample from a rigid transformation then? In order to sample from a multivariate Gaussian, we have to sample in a vector space. Luckily, rigid motion velocities $v$ are vectors (called twist)[edit: well, at least twist is a pseudo-vector. If somebody could provide more insight concerning the implications and underlying assumptions of considering a Gaussian over twist; that would be great]. The vector $v=(\nu, \omega)$ is 6-dimensional with $\nu$ being a translational and $\omega$ a rotational 3-vector. This velocity can be related to rigid motion $T$ using the exponential map $\exp(v)=T$. The interpretation is the following: Starting from the identity, we end up at T if we move with translational/rotational velocity v (for a unit of time).

Now, let us consider our correct rigid motion $\hat{T}_{a,b}$ from frame $b$ to frame $a$. A set of approximations $T^{(1)}_{a,b},...,T^{(n)}_{a,b}$ can be produced by $T^{(i)}_{a,b} = \hat{T_{a,b}}\cdot\exp(v_b)$ with $v_b$ a velocity in frame $b$ drawn from the zero-mean Gaussian ${\cal N}(0,\Sigma_b)$.

Problem formulation

The problem can be formulated as the following generalized least-squares problem:

Minimizing $\sum_i v_i^{T}\Sigma_b^{-1}v_i$ with $v_i:=\log((T^{(i)}_{a,b})^{-1}T_{a,b}(x))$ with respect to $T_{a,b}$. Here, $\log$ is the logarithmic map of the group of rigid motion (the inverse of $\exp(\cdot)$).

How to minimize with respect to the a rigid transformation $T_{a,b}$? We can iteratively solve for x, with $T^{new}_{a,b}= T^{old}_{a,b}\exp(x)$.

-