A method, including: receiving an input indicative of respective apparent locations of a plurality of points disposed along a length of a probe inside a body of a subject, and applying a model of known mechanical properties of the probe to the respective apparent locations so as to minimize a first cost function with respect to shapes that can be assumed by the probe in the body. The method further includes choosing a shape responsively to the minimized first cost function and determining preliminary coordinates of the apparent locations responsively to the shape, minimizing a second cost function with respect to differences between the apparent locations and the preliminary coordinates, and generating corrected coordinates of the points along the length of the probe based on the minimized second cost function.