ICP Working ok with 10 or 20 captures, until drift appears. g2o integrated to combat drift