Update KDL SegmentMap interface to use shared pointers