By using Manhattan distance, a penalty function is constructed by weighting the distance from the safe position to the camera and the distance from the current position, incentivizing safe positions to move away from the camera. The isometric line of the integrated Manhattan distance changes from a circle to a trapezoidal shape, with the Euclidean distance toward the camera being half that away from the camera. This ensures that when selecting a safe position, points with the same Euclidean distance from the current position receive a smaller penalty if they are farther away from the camera.
jira: none
Change-Id: I3c256cfc0c33cecade86c254613063adeac59f91
(cherry picked from commit f86ca36b471dd99d4c1bd21c4e6db3ee10eb5a6d)
eafe57d8c6