ZED world coordinate to camera coordinates

I figured it out.

// get camera pose wrt world frame
zed.getPosition(cam_pose, REFERENCE_FRAME::WORLD);
auto cam_to_world = cam_pose;
cam_to_world.pose_data.inverse();
// current_pos is the position of an object wrt world frame, new_pos is wrt to camera frame
sl::Translation new_pos = current_pos * cam_to_world.getOrientation() + cam_to_world.getTranslation();
1 Like