Optimized ICP a bit