Update BFSMulti

- Add Global Rigid ICP
- Refine BFSMulti::init()