function dist = DistBtn2Dpose(pose1, pose2)
dist = sqrt( (pose1(1) - pose2(1))^2 + (pose1(2) - pose2(2))^2);
end