LOAM

LOAM Optimization

Reading memo of Dr. Zhang’s paper. Core algorithm for pose estimation are these equations.

Point to line cost

$v_1=p_0-p_1$

$v_2=p_0-p_2$

$v_3=p_1-p_2$

$a_{012} = |(y_1 z_2 - y_2 z_1, x_1 z_2-x_2 z_1, x_1 y_2 - x_2 y_1)|$

$l_{12} = |(x_3,y_3,z_3)|$

$la = \frac{y_3(x_1 y_2 - x_2 y_1) + z_3(x_1 z_2 - x_2 z_1)}{a{012} l_{12}}$

$lb = -\frac{x_3(x_1 y_2 - x_2 y_1) - z_3(y_1 z_2 - y_2 z_1)} {a{012} l_{12}}$

$lc = -\frac{x_3(x_1 z_2 - x_2 z_1) + y_3(y_1 z_2 - y_2 z_1) }{a{012} l_{12}}$

$l{d2} = \frac{a{012}}{l_{12}}$

Point to plane cost

Source Point $p_0$
Three points in reference $p_1$ $p_2$ $p_3$

$v_1=p_2-p_1$

$v_2=p_3-p_1$

$pa = y_1 z_2 - y_2 z_1$

$pb = z_1 x_2 - z_2 x_1$

$pc = x_1 y_2 - x_2 y_1$

$pd = -(pa, pb, pc) * p1 = - (v_1 \times v_2) \cdot p_1$

$ps = |pd|$

$pd2 = pa \cdot x_0 + pb \cdot y_0 + pc \cdot z_0 +pd$