A Two-stage Consensus Filtering for Real-time 3D Registration - View it on GitHub