A shape sensing system to determine the position and orientation of one link with respect to another link in a kinematic chain. An optical fiber is coupled to two or more links in a kinematic chain. A shape sensing segment is defined to start at a proximal link and to end at a distal link, crossing one or more joints. A reference frame is defined at the start of the shape sensing segment. As the joints move, an interrogator senses strain in the shape sensing segment. The sensed strain is used to output a Cartesian position and orientation of the end of the shape sensing segment with respect to the reference frame defined at the start of the shape sensing segment. The pose of the kinematic chain is determined from the Cartesian positions and orientations of one or more shape sensing segments defined for the kinematic chain and from an a priori model and constraints of the kinematic chain.