Add markers showing id of calibration targets

This PR adds additional markers with the id of the calibration targets used for optimization. This can come in very handy when watching optimization progress in rviz for picking out the "bad apple" targets that prevent the system from converging.