Iterative Closest Point (ICP) é um algoritmo que utiliza-se de imagens de profundidade (RGB-D) para encontrar a pose inicial da câmera em relação ao mundo de forma que seja possível empilhar diversas nuvens de ponto corretamente posicionadas e, então, ter um modelo 3D da cena.
O ICP e suas variações são propiciosos nesses casos pois as coordenadas 3D que o sensor emite permitem associar os pontos e assim estimar uma matriz de pose.
https://www.youtube.com/watch?v=QWDM4cFdKrE
Uma das ferramentas matemáticas usando pelo ICP quando formulado como least-squares é o uso de Lie Algebra para representar transformações, principalmente para obter as derivadas das transformações.