Fix: fitness update error on icp.cpp for in_hand_scanner.

Moved update of previous_fitness to after it has been used.