changed the way semi_cost and final_cost were allocated. Using an array of pointers to opencv 2d matrix instead of using 3d opencv Mat