Fix 3D distance calculation formula
Fix 3D distance calculation formula


Former-commit-id: d19eeb059b738fbf5c02fc9dc5631e53ac336a6e
valkyrierobotics/vision2017,valkyrierobotics/vision2017,valkyrierobotics/vision2017