First of all: great initiative, and I’d be happy to try this implementation.
I’ve had a short look through your code, so please correct me if I missed pieces.
Is it right that, once you’ve seen a marker, its position will not be updated again?
The fiducial marker SLAM problem strikes me as a perfect case for Graph SLAM. Is there any reason you are not using this?