Update costOptimization.hpp