I have been trying to use Iterative Closest Point algorithm to minimize the distance between a point and a line for my scan matching project (SLAM).
Here is rather a simpler explanation. I am given $N$ sets of three points. $p$ is from time $t-1$, whereas the $q_1$ & $q_2$ are from time $t$. I have used KD tree search to find $q_1$ & $q_2$ that are supposedly "closest" to $p$.
I have sourced least squares optimization problem to obtain Rotation & Translation vector that would minimize the sum of distance between them.
Put mathematically,
The goal is to minimize error function: $$ E(R, t) = \sum_{i}\lVert (Rp_i + t - q_i) \cdot n_i^q \rVert^2$$ where $R$ & $t$ are rotation and translation matrix, $p$ is the point being adjusted, and $q$ is the reference point where $q$ would have to match. $n$ is the normal vector from the line connecting $q_1$ and $q_2$. (I am slightly confused on the notation here since there are 2 $q$'s)
After small angle approximation and rearranging terms into $Ax = B$ to solve for $x$, here are the 3D least squares optimization process. I need to solve for $x$ where the matrix would iteratively return [roll, pitch, yaw, and translation in $xyz$] to minimize the cost function between point and a line. For my case, the problem needs to be simplified into $3 \times 1$ matrix instead of $6 \times 1$ since I would only need [yaw, tx, ty].
$$ Ax = B $$ It can be expressed in system of $N$ linear equations, where $N$ is no. of corresponding points between $p$ & $q$. $$Row_i\ of\ A = [(p_i \times n_i^q)_x, (p_i \times n_i^q)_y, (p_i \times n_i^q)_z, (n_i^q)_x, (n_i^q)_y, (n_i^q)_z]$$
$$ x = [\alpha, \beta, \gamma, tx, ty, tz]^T $$ $$ Row_i\ of\ B = [(p_i - q_i)\cdot n^q_i] $$
Here, $x$ can be solved by $$ x = (A^TA)^{-1} (A^Tb) $$
Basically, I have equations in 3D coordinates to solve for $x$. But I am having difficulty converting this in to train of thoughts that would yield me 2D equations to code.
- I think the confusion is coming from the fact that I am not sure how to set up $A$ & $B$ in 2D situations.
- I am confused with how to work with two $q$'s that are given per 1 $p$.