Since we sample particles for PST within a unit sphere in the 6D state space, it is guaranteed that any sampled particle represents a valid rigid body motion in \(SE(3)\) . The translation is measured in meter, hence each optimization step could explore a maximum range of 1 meter for translation and \(2\pi \)  for rotation. To realize an unbiased particle sampling, we adopt the 6D Poisson disk sampling proposed in [1]}.
