Changed the code so only markers not attached to a rigid body are added to the labeledMarkers map. Also labeledMarkers are saved during the recording.