Move some routines to solver_utils.h/cc This moves a couple of routines from solver.cc into solver_utils.h/cc so that they can also be used by the upcoming GradientProblemSolver. Change-Id: I627b32ad3dc639422aacde78a8e391459d947e99