A system for measuring the position of a hand-held tool relative to a body in five or six degrees of freedom is provided. The system comprises a rigid or semi-flexible tool with a distal end for insertion into the body and a proximal portion for manual manipulation outside the body, and a flexible patch carrying a plurality of markers, wherein the patch is configured to be applied or attached to an external surface of the body. The system also comprises a camera for generating images and a mechanical linkage is configured to attach said camera to the proximal portion of said tool such that said camera moves together with said proximal portion of said tool, and said camera is directed with a field of view toward at least part of said distal end of said tool. The field of view includes the markers when the patch is applied or attached to the external surface of the body and the distal end of the tool is inserted into the body. The system further comprises a processing system in data communication with said camera and configured to obtain from the camera images of the external surface of the body including the plurality of said markers, and configured to process the images from said camera to determine a position of at last part of said tool relative to the markers. Furthermore, a method for guiding a distal end of a rigid or semi-flexible tool to a target within a body is provided.