6D pose and twist estimatio