Fork of GTSAM with some nice PCL compatibility