Optimized and tweak the point finder algorithm.

Signed-off-by: Kristian Duske <kristian.duske@gmail.com>
Some refactoring.

Signed-off-by: Kristian Duske <kristian.duske@gmail.com>
Integration of plane point algorithm

Signed-off-by: Kristian Duske <kristian.duske@gmail.com>