FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects