The mobile robot programming toolkit (mrpt) is an extensive, cross-platform, and open source c++ library aimed to help robotics researchers to design and implement algorithms in the fields of simultaneous localization and mapping (slam, graph-slam, bundle adjustment), computer vision, and motion planning (obstacle avoidance).

this package includes the mrpt-tfest library.

more about mrpt libraries in: http://www.mrpt.org/libraries