diff --git a/include/laser/processor/params_icp.h b/include/laser/processor/params_icp.h
index 52f5f66112b7bff9766fc945722f35c53ba154c7..7379a8029c7ed9955de20d3cc16fa75e20261d7e 100644
--- a/include/laser/processor/params_icp.h
+++ b/include/laser/processor/params_icp.h
@@ -94,36 +94,36 @@ struct ParamsIcp
 
     std::string print() const
     {
-        return "/icp/verbose" + std::to_string(icp_params.verbose) + "\n" + "/icp/use_point_to_line_distance" +
-               std::to_string(icp_params.use_point_to_line_distance) + "\n" + "/icp/max_angular_correction_deg" +
-               std::to_string(icp_params.max_angular_correction_deg) + "\n" + "/icp/max_linear_correction" +
-               std::to_string(icp_params.max_linear_correction) + "\n" + "/icp/max_iterations" +
-               std::to_string(icp_params.max_iterations) + "\n" + "/icp/epsilon_xy" +
-               std::to_string(icp_params.epsilon_xy) + "\n" + "/icp/epsilon_theta" +
-               std::to_string(icp_params.epsilon_theta) + "\n" + "/icp/max_correspondence_dist" +
-               std::to_string(icp_params.max_correspondence_dist) + "\n" + "/icp/use_corr_tricks" +
-               std::to_string(icp_params.use_corr_tricks) + "\n" + "/icp/debug_verify_tricks" +
-               std::to_string(icp_params.debug_verify_tricks) + "\n" + "/icp/restart" +
-               std::to_string(icp_params.restart) + "\n" + "/icp/restart_threshold_mean_error" +
-               std::to_string(icp_params.restart_threshold_mean_error) + "\n" + "/icp/restart_dt" +
-               std::to_string(icp_params.restart_dt) + "\n" + "/icp/restart_dtheta" +
-               std::to_string(icp_params.restart_dtheta) + "\n" + "/icp/min_reading" +
-               std::to_string(icp_params.min_reading) + "\n" + "/icp/max_reading" +
-               std::to_string(icp_params.max_reading) + "\n" + "/icp/outliers_maxPerc" +
-               std::to_string(icp_params.outliers_maxPerc) + "\n" + "/icp/outliers_adaptive_order" +
-               std::to_string(icp_params.outliers_adaptive_order) + "\n" + "/icp/outliers_adaptive_mult" +
-               std::to_string(icp_params.outliers_adaptive_mult) + "\n" + "/icp/outliers_remove_doubles" +
-               std::to_string(icp_params.outliers_remove_doubles) + "\n" + "/icp/do_visibility_test" +
-               std::to_string(icp_params.do_visibility_test) + "\n" + "/icp/do_alpha_test" +
-               std::to_string(icp_params.do_alpha_test) + "\n" + "/icp/do_alpha_test_thresholdDeg" +
-               std::to_string(icp_params.do_alpha_test_thresholdDeg) + "\n" + "/icp/clustering_threshold" +
-               std::to_string(icp_params.clustering_threshold) + "\n" + "/icp/orientation_neighbourhood" +
-               std::to_string(icp_params.orientation_neighbourhood) + "\n" + "/icp/use_ml_weights" +
-               std::to_string(icp_params.use_ml_weights) + "\n" + "/icp/use_sigma_weights" +
-               std::to_string(icp_params.use_sigma_weights) + "\n" + "/icp/sigma" + std::to_string(icp_params.sigma) +
-               "\n" + "/icp/do_compute_covariance" + std::to_string(icp_params.do_compute_covariance) + "\n" +
-               "/icp/cov_factor" + std::to_string(icp_params.cov_factor) + "\n" + "/icp/cov_max_eigv_factor" +
-               std::to_string(icp_params.cov_max_eigv_factor) + "\n";
+        return "/icp/verbose" + toString(icp_params.verbose) + "\n" + "/icp/use_point_to_line_distance" +
+               toString(icp_params.use_point_to_line_distance) + "\n" + "/icp/max_angular_correction_deg" +
+               toString(icp_params.max_angular_correction_deg) + "\n" + "/icp/max_linear_correction" +
+               toString(icp_params.max_linear_correction) + "\n" + "/icp/max_iterations" +
+               toString(icp_params.max_iterations) + "\n" + "/icp/epsilon_xy" +
+               toString(icp_params.epsilon_xy) + "\n" + "/icp/epsilon_theta" +
+               toString(icp_params.epsilon_theta) + "\n" + "/icp/max_correspondence_dist" +
+               toString(icp_params.max_correspondence_dist) + "\n" + "/icp/use_corr_tricks" +
+               toString(icp_params.use_corr_tricks) + "\n" + "/icp/debug_verify_tricks" +
+               toString(icp_params.debug_verify_tricks) + "\n" + "/icp/restart" +
+               toString(icp_params.restart) + "\n" + "/icp/restart_threshold_mean_error" +
+               toString(icp_params.restart_threshold_mean_error) + "\n" + "/icp/restart_dt" +
+               toString(icp_params.restart_dt) + "\n" + "/icp/restart_dtheta" +
+               toString(icp_params.restart_dtheta) + "\n" + "/icp/min_reading" +
+               toString(icp_params.min_reading) + "\n" + "/icp/max_reading" +
+               toString(icp_params.max_reading) + "\n" + "/icp/outliers_maxPerc" +
+               toString(icp_params.outliers_maxPerc) + "\n" + "/icp/outliers_adaptive_order" +
+               toString(icp_params.outliers_adaptive_order) + "\n" + "/icp/outliers_adaptive_mult" +
+               toString(icp_params.outliers_adaptive_mult) + "\n" + "/icp/outliers_remove_doubles" +
+               toString(icp_params.outliers_remove_doubles) + "\n" + "/icp/do_visibility_test" +
+               toString(icp_params.do_visibility_test) + "\n" + "/icp/do_alpha_test" +
+               toString(icp_params.do_alpha_test) + "\n" + "/icp/do_alpha_test_thresholdDeg" +
+               toString(icp_params.do_alpha_test_thresholdDeg) + "\n" + "/icp/clustering_threshold" +
+               toString(icp_params.clustering_threshold) + "\n" + "/icp/orientation_neighbourhood" +
+               toString(icp_params.orientation_neighbourhood) + "\n" + "/icp/use_ml_weights" +
+               toString(icp_params.use_ml_weights) + "\n" + "/icp/use_sigma_weights" +
+               toString(icp_params.use_sigma_weights) + "\n" + "/icp/sigma" + toString(icp_params.sigma) +
+               "\n" + "/icp/do_compute_covariance" + toString(icp_params.do_compute_covariance) + "\n" +
+               "/icp/cov_factor" + toString(icp_params.cov_factor) + "\n" + "/icp/cov_max_eigv_factor" +
+               toString(icp_params.cov_max_eigv_factor) + "\n";
     }
 };
 
diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h
index 1fa38e334230f8b5c36b2db5642a178c54688d27..85c658f680a08213fee206e470c4b35a6a315447 100644
--- a/include/laser/processor/processor_loop_closure_falko.h
+++ b/include/laser/processor/processor_loop_closure_falko.h
@@ -116,33 +116,33 @@ struct ParamsProcessorLoopClosureFalko : public ParamsProcessorLoopClosure
 
     std::string print() const override
     {
-        return ParamsProcessorLoopClosure::print() + "\n" + "min_keypoints: " + std::to_string(min_keypoints) + "\n" +
-               "score_th: " + std::to_string(score_th) + "\n" +
-               "distance_between_ref_frm: " + std::to_string(distance_between_ref_frm) + "\n" +
-               "near_matches_th: " + std::to_string(near_matches_th) + "\n" +
-               "th_kp_area: " + std::to_string(th_kp_area) + "\n" +
-               "th_kp_perimeter: " + std::to_string(th_kp_perimeter) + "\n" +
-               "th_kp_maxd: " + std::to_string(th_kp_maxd) + "\n" + "th_kp_eig: " + std::to_string(th_kp_eig) + "\n" +
-               "th_scan_area: " + std::to_string(th_scan_area) + "\n" + "th_scan_eig: " + std::to_string(th_scan_eig) +
-               "\n" + "logging: " + std::to_string(logging) + "\n" + "logging_filepath: " + logging_filepath + "\n"
-
-               + "falko/matcher_distance_th_: " + std::to_string(param.matcher_distance_th_) + "\n" +
-               "falko/use_descriptors_: " + std::to_string(param.use_descriptors_) + "\n" +
-               "falko/circularSectorNumber_: " + std::to_string(param.circularSectorNumber_) + "\n" +
-               "falko/radialRingNumber_: " + std::to_string(param.radialRingNumber_) + "\n" +
-               "falko/matcher_ddesc_th_: " + std::to_string(param.matcher_ddesc_th_) + "\n" +
-               "falko/keypoints_number_th_: " + std::to_string(param.keypoints_number_th_) + "\n"
-
-               + "falko/enable_subbeam_: " + std::to_string(param.enable_subbeam_) + "\n" +
-               "falko/nms_radius_: " + std::to_string(param.nms_radius_) + "\n" +
-               "falko/neigh_b_: " + std::to_string(param.neigh_b_) + "\n" +
-               "falko/b_ratio_: " + std::to_string(param.b_ratio_) + "\n"
-
-               + "falko/xRes_: " + std::to_string(param.xRes_) + "\n" + "falko/yRes_: " + std::to_string(param.yRes_) +
-               "\n" + "falko/thetaRes_: " + std::to_string(param.thetaRes_) + "\n" +
-               "falko/xAbsMax_: " + std::to_string(param.xAbsMax_) + "\n" +
-               "falko/yAbsMax_: " + std::to_string(param.yAbsMax_) + "\n" +
-               "falko/thetaAbsMax_: " + std::to_string(param.thetaAbsMax_) + "\n";
+        return ParamsProcessorLoopClosure::print() + "\n" + "min_keypoints: " + toString(min_keypoints) + "\n" +
+               "score_th: " + toString(score_th) + "\n" +
+               "distance_between_ref_frm: " + toString(distance_between_ref_frm) + "\n" +
+               "near_matches_th: " + toString(near_matches_th) + "\n" +
+               "th_kp_area: " + toString(th_kp_area) + "\n" +
+               "th_kp_perimeter: " + toString(th_kp_perimeter) + "\n" +
+               "th_kp_maxd: " + toString(th_kp_maxd) + "\n" + "th_kp_eig: " + toString(th_kp_eig) + "\n" +
+               "th_scan_area: " + toString(th_scan_area) + "\n" + "th_scan_eig: " + toString(th_scan_eig) +
+               "\n" + "logging: " + toString(logging) + "\n" + "logging_filepath: " + logging_filepath + "\n"
+
+               + "falko/matcher_distance_th_: " + toString(param.matcher_distance_th_) + "\n" +
+               "falko/use_descriptors_: " + toString(param.use_descriptors_) + "\n" +
+               "falko/circularSectorNumber_: " + toString(param.circularSectorNumber_) + "\n" +
+               "falko/radialRingNumber_: " + toString(param.radialRingNumber_) + "\n" +
+               "falko/matcher_ddesc_th_: " + toString(param.matcher_ddesc_th_) + "\n" +
+               "falko/keypoints_number_th_: " + toString(param.keypoints_number_th_) + "\n"
+
+               + "falko/enable_subbeam_: " + toString(param.enable_subbeam_) + "\n" +
+               "falko/nms_radius_: " + toString(param.nms_radius_) + "\n" +
+               "falko/neigh_b_: " + toString(param.neigh_b_) + "\n" +
+               "falko/b_ratio_: " + toString(param.b_ratio_) + "\n"
+
+               + "falko/xRes_: " + toString(param.xRes_) + "\n" + "falko/yRes_: " + toString(param.yRes_) +
+               "\n" + "falko/thetaRes_: " + toString(param.thetaRes_) + "\n" +
+               "falko/xAbsMax_: " + toString(param.xAbsMax_) + "\n" +
+               "falko/yAbsMax_: " + toString(param.yAbsMax_) + "\n" +
+               "falko/thetaAbsMax_: " + toString(param.thetaAbsMax_) + "\n";
     };
 };
 
@@ -217,7 +217,7 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
         std::string th_scan_eig(ss12.str());
 
         std::string filename = params_falko_->logging_filepath + "/log" + "_use_descriptors_" +
-                               std::to_string(params_falko_->param.use_descriptors_) + "_score_th_" + score_th +
+                               toString(params_falko_->param.use_descriptors_) + "_score_th_" + score_th +
                                "_th_kp_area_" + th_kp_area + "_th_kp_perimeter_" + th_kp_perimeter + "_th_kp_maxd_" +
                                th_kp_maxd + "_th_kp_eig_" + th_kp_eig + "_th_scan_area_" + th_scan_area +
                                "_th_scan_eig_" + th_scan_eig + "_matcher_distance_th_" + matcher_distance_th +
@@ -535,21 +535,21 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
     {
         if (init_outData_ == 1)
         {
-            outdata << std::to_string(num_ok_) << ";";
-            outdata << std::to_string(num_nok_) << ";";
+            outdata << toString(num_ok_) << ";";
+            outdata << toString(num_nok_) << ";";
             if (num_ok_ > 0)
             {
-                outdata << std::to_string(duration_validation_ok.count() / num_ok_) << ";";
+                outdata << toString(duration_validation_ok.count() / num_ok_) << ";";
             }
             else
-                outdata << std::to_string(0) << ";";
+                outdata << toString(0) << ";";
 
             if (num_nok_ > 0)
             {
-                outdata << std::to_string(duration_validation_nok.count() / num_nok_) << ";" << std::endl;
+                outdata << toString(duration_validation_nok.count() / num_nok_) << ";" << std::endl;
             }
             else
-                outdata << std::to_string(0) << ";" << std::endl;
+                outdata << toString(0) << ";" << std::endl;
             num_ok_                 = 0;
             num_nok_                = 0;
             duration_validation_ok  = std::chrono::duration_values<std::chrono::microseconds>::zero();
@@ -561,20 +561,20 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
             num_nok_ = 0;
         }
         init_outData_ = 1;
-        outdata << std::to_string(list_scene_reference.size()) << ";";
-        outdata << std::to_string(num_ref_scenes) << ";";
-        outdata << std::to_string(match_lc_map.size()) << ";";
-        outdata << std::to_string(keypoints_list.size()) << ";";
+        outdata << toString(list_scene_reference.size()) << ";";
+        outdata << toString(num_ref_scenes) << ";";
+        outdata << toString(match_lc_map.size()) << ";";
+        outdata << toString(keypoints_list.size()) << ";";
         if (match_lc_map.size() > 0)
         {
-            outdata << std::to_string(acum_score / match_lc_map.size()) << ";";
+            outdata << toString(acum_score / match_lc_map.size()) << ";";
         }
         else
         {
-            outdata << std::to_string(0) << ";";
+            outdata << toString(0) << ";";
         }
-        outdata << std::to_string(duration_find_loop.count()) << ";";
-        outdata << std::to_string(duration_extract_scene.count()) << ";";
+        outdata << toString(duration_find_loop.count()) << ";";
+        outdata << toString(duration_extract_scene.count()) << ";";
     }
 
     ParamsProcessorLoopClosureFalkoPtr params_falko_;
diff --git a/include/laser/processor/processor_loop_closure_falko_icp.h b/include/laser/processor/processor_loop_closure_falko_icp.h
index efb8456d825c97121da84c1caf4f9aea9e2a2999..b945e00c365ac39810e5199b8a085bfed45d238d 100644
--- a/include/laser/processor/processor_loop_closure_falko_icp.h
+++ b/include/laser/processor/processor_loop_closure_falko_icp.h
@@ -57,8 +57,8 @@ struct ParamsProcessorLoopClosureFalkoIcp : public ParamsProcessorLoopClosureFal
     std::string print() const override
     {
         return "\n" + ParamsProcessorLoopClosureFalko::print() + "\n" + ParamsIcp::print() + "\n" +
-               "max_error_threshold: " + std::to_string(max_error_threshold) + "\n" +
-               "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+               "max_error_threshold: " + toString(max_error_threshold) + "\n" +
+               "min_points_percent: " + toString(min_points_percent) + "\n";
     }
 };
 
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index 6e55b23a9e29b843c0232494f41674c7935a9c9b..5f3285508152dc18f96284020b043789adfa7554 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -70,13 +70,13 @@ struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure, public
     std::string print() const override
     {
         return "\n" + ParamsProcessorLoopClosure::print() + ParamsIcp::print() + "\n" +
-               "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n" +
-               "frames_ignored_after_loop: " + std::to_string(frames_ignored_after_loop) + "\n" +
-               "max_attempts: " + std::to_string(max_attempts) + "\n" +
-               "max_candidates: " + std::to_string(max_candidates) + "\n" +
+               "recent_frames_ignored: " + toString(recent_frames_ignored) + "\n" +
+               "frames_ignored_after_loop: " + toString(frames_ignored_after_loop) + "\n" +
+               "max_attempts: " + toString(max_attempts) + "\n" +
+               "max_candidates: " + toString(max_candidates) + "\n" +
                "candidate_generation: " + candidate_generation + "\n" +
-               "max_error_threshold: " + std::to_string(max_error_threshold) + "\n" +
-               "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+               "max_error_threshold: " + toString(max_error_threshold) + "\n" +
+               "min_points_percent: " + toString(min_points_percent) + "\n";
     }
 };
 
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index beb790b6112c93ee54cfe45c9043fe67932cc7d0..228d6703bf02eb8821e61808af9c62f85269f030 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -72,11 +72,11 @@ struct ParamsProcessorOdomIcp : public ParamsProcessorTracker, public ParamsMoti
     {
         return "\n" + ParamsProcessorTracker::print() + "\n" + ParamsMotionProvider::print() + "\n" +
                ParamsIcp::print() + "\n" + "initial_guess: " + initial_guess + "\n" +
-               "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\n" +
-               "keyframe_vote/min_angle: " + std::to_string(vfk_min_angle) + "\n" +
-               "keyframe_vote/min_time: " + std::to_string(vfk_min_time) + "\n" +
-               "keyframe_vote/min_error: " + std::to_string(vfk_min_error) + "\n" +
-               "keyframe_vote/max_points: " + std::to_string(vfk_max_points) + "\n";
+               "keyframe_vote/min_dist: " + toString(vfk_min_dist) + "\n" +
+               "keyframe_vote/min_angle: " + toString(vfk_min_angle) + "\n" +
+               "keyframe_vote/min_time: " + toString(vfk_min_time) + "\n" +
+               "keyframe_vote/min_error: " + toString(vfk_min_error) + "\n" +
+               "keyframe_vote/max_points: " + toString(vfk_max_points) + "\n";
     }
 };
 
diff --git a/include/laser/sensor/sensor_laser_2d.h b/include/laser/sensor/sensor_laser_2d.h
index 9888fabaf3b8d72008c343c6b9d852909590d086..e6109168eee9073941257e3cebfd1385f5d9e9cb 100644
--- a/include/laser/sensor/sensor_laser_2d.h
+++ b/include/laser/sensor/sensor_laser_2d.h
@@ -58,14 +58,14 @@ struct ParamsSensorLaser2d : public ParamsSensorBase
     std::string print() const override
     {
         return "\n" + ParamsSensorBase::print() + "\n" +
-               "LaserScanParams/angle_min: " + std::to_string(scan_params.angle_min_) + "\n" +
-               "LaserScanParams/angle_max: " + std::to_string(scan_params.angle_max_) + "\n" +
-               "LaserScanParams/angle_step: " + std::to_string(scan_params.angle_step_) + "\n" +
-               "LaserScanParams/scan_time: " + std::to_string(scan_params.scan_time_) + "\n" +
-               "LaserScanParams/range_min: " + std::to_string(scan_params.range_min_) + "\n" +
-               "LaserScanParams/range_max: " + std::to_string(scan_params.range_max_) + "\n" +
-               "LaserScanParams/range_std_dev: " + std::to_string(scan_params.range_std_dev_) + "\n" +
-               "LaserScanParams/angle_std_dev: " + std::to_string(scan_params.angle_std_dev_) + "\n";
+               "LaserScanParams/angle_min: " + toString(scan_params.angle_min_) + "\n" +
+               "LaserScanParams/angle_max: " + toString(scan_params.angle_max_) + "\n" +
+               "LaserScanParams/angle_step: " + toString(scan_params.angle_step_) + "\n" +
+               "LaserScanParams/scan_time: " + toString(scan_params.scan_time_) + "\n" +
+               "LaserScanParams/range_min: " + toString(scan_params.range_min_) + "\n" +
+               "LaserScanParams/range_max: " + toString(scan_params.range_max_) + "\n" +
+               "LaserScanParams/range_std_dev: " + toString(scan_params.range_std_dev_) + "\n" +
+               "LaserScanParams/angle_std_dev: " + toString(scan_params.angle_std_dev_) + "\n";
     }
 };