Fork of GTSAM with some nice PCL compatibility - View it on GitHub