diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt
index 7fa4e6735..bba5c3e78 100644
--- a/ov_core/CMakeLists.txt
+++ b/ov_core/CMakeLists.txt
@@ -91,6 +91,7 @@ add_library(ov_core_lib SHARED
         src/types/Landmark.cpp
         src/feat/Feature.cpp
         src/feat/FeatureInitializer.cpp
+        src/utils/print.cpp
 )
 target_link_libraries(ov_core_lib ${thirdparty_libraries})
 target_include_directories(ov_core_lib PUBLIC src)
diff --git a/ov_core/src/feat/FeatureDatabase.h b/ov_core/src/feat/FeatureDatabase.h
index d272dc997..6580f3434 100644
--- a/ov_core/src/feat/FeatureDatabase.h
+++ b/ov_core/src/feat/FeatureDatabase.h
@@ -28,6 +28,7 @@
 #include <vector>
 
 #include "Feature.h"
+#include "utils/print.h"
 
 namespace ov_core {
 
@@ -159,7 +160,7 @@ class FeatureDatabase {
     }
 
     // Debugging
-    // std::cout << "feature db size = " << features_idlookup.size() << std::endl;
+    // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size())
 
     // Return the old features
     return feats_old;
@@ -206,7 +207,7 @@ class FeatureDatabase {
     }
 
     // Debugging
-    // std::cout << "feature db size = " << features_idlookup.size() << std::endl;
+    // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size())
 
     // Return the old features
     return feats_old;
@@ -253,8 +254,8 @@ class FeatureDatabase {
     }
 
     // Debugging
-    // std::cout << "feature db size = " << features_idlookup.size() << std::endl;
-    // std::cout << "return vector = " << feats_has_timestamp.size() << std::endl;
+    // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size())
+    // PRINT_DEBUG("return vector = %u\n", feats_has_timestamp.size())
 
     // Return the features
     return feats_has_timestamp;
@@ -277,7 +278,7 @@ class FeatureDatabase {
         it++;
       }
     }
-    // std::cout << "feat db = " << sizebefore << " -> " << (int)features_idlookup.size() << std::endl;
+    // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl;
   }
 
   /**
@@ -386,7 +387,7 @@ class FeatureDatabase {
         features_idlookup[feat.first] = temp;
       }
     }
-    // std::cout << "feat db = " << sizebefore << " -> " << (int)features_idlookup.size() << std::endl;
+    // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl;
   }
 
 protected:
diff --git a/ov_core/src/feat/FeatureInitializer.cpp b/ov_core/src/feat/FeatureInitializer.cpp
index f176c5c98..5e92098bc 100644
--- a/ov_core/src/feat/FeatureInitializer.cpp
+++ b/ov_core/src/feat/FeatureInitializer.cpp
@@ -20,6 +20,9 @@
  */
 
 #include "FeatureInitializer.h"
+#include "utils/print.h"
+
+#include <sstream>
 
 using namespace ov_core;
 
@@ -88,7 +91,10 @@ bool FeatureInitializer::single_triangulation(Feature *feat, std::unordered_map<
   singularValues.resize(svd.singularValues().rows(), 1);
   singularValues = svd.singularValues();
   double condA = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);
-  // std::cout << feat->featid << " - cond " << std::abs(condA) << " - z " << p_f(2, 0) << std::endl;
+
+  // std::stringstream ss;
+  // ss << feat->featid << " - cond " << std::abs(condA) << " - z " << p_f(2, 0) << std::endl;
+  // PRINT_DEBUG(ss.str().c_str());
 
   // If we have a bad condition number, or it is too close
   // Then set the flag for bad (i.e. set z-axis to nan)
@@ -289,7 +295,9 @@ bool FeatureInitializer::single_gaussnewton(Feature *feat, std::unordered_map<si
     double cost = compute_error(clonesCAM, feat, alpha + dx(0, 0), beta + dx(1, 0), rho + dx(2, 0));
 
     // Debug print
-    // cout << "run = " << runs << " | cost = " << dx.norm() << " | lamda = " << lam << " | depth = " << 1/rho << endl;
+    // std::stringstream ss;
+    // ss << "run = " << runs << " | cost = " << dx.norm() << " | lamda = " << lam << " | depth = " << 1/rho << endl;
+    // PRINT_DEBUG(ss.str().c_str());
 
     // Check if converged
     if (cost <= cost_old && (cost_old - cost) / cost_old < _options.min_dcost) {
@@ -345,7 +353,9 @@ bool FeatureInitializer::single_gaussnewton(Feature *feat, std::unordered_map<si
         base_line_max = base_line;
     }
   }
-  // std::cout << feat->featid << " - max base " << (feat->p_FinA.norm() / base_line_max) << " - z " << feat->p_FinA(2) << std::endl;
+  // std::stringstream ss;
+  // ss << feat->featid << " - max base " << (feat->p_FinA.norm() / base_line_max) << " - z " << feat->p_FinA(2) << std::endl;
+  // PRINT_DEBUG(ss.str().c_str());
 
   // Check if this feature is bad or not
   // 1. If the feature is too close
diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h
index bb83f04a7..352292b71 100644
--- a/ov_core/src/feat/FeatureInitializerOptions.h
+++ b/ov_core/src/feat/FeatureInitializerOptions.h
@@ -22,6 +22,8 @@
 #ifndef OV_CORE_INITIALIZEROPTIONS_H
 #define OV_CORE_INITIALIZEROPTIONS_H
 
+#include "utils/print.h"
+
 namespace ov_core {
 
 /**
@@ -67,18 +69,18 @@ struct FeatureInitializerOptions {
 
   /// Nice print function of what parameters we have loaded
   void print() {
-    printf("\t- triangulate_1d: %d\n", triangulate_1d);
-    printf("\t- refine_features: %d\n", refine_features);
-    printf("\t- max_runs: %d\n", max_runs);
-    printf("\t- init_lamda: %.3f\n", init_lamda);
-    printf("\t- max_lamda: %.3f\n", max_lamda);
-    printf("\t- min_dx: %.7f\n", min_dx);
-    printf("\t- min_dcost: %.7f\n", min_dcost);
-    printf("\t- lam_mult: %.3f\n", lam_mult);
-    printf("\t- min_dist: %.3f\n", min_dist);
-    printf("\t- max_dist: %.3f\n", max_dist);
-    printf("\t- max_baseline: %.3f\n", max_baseline);
-    printf("\t- max_cond_number: %.3f\n", max_cond_number);
+    PRINT_INFO("\t- triangulate_1d: %d\n", triangulate_1d);
+    PRINT_INFO("\t- refine_features: %d\n", refine_features);
+    PRINT_INFO("\t- max_runs: %d\n", max_runs);
+    PRINT_INFO("\t- init_lamda: %.3f\n", init_lamda);
+    PRINT_INFO("\t- max_lamda: %.3f\n", max_lamda);
+    PRINT_INFO("\t- min_dx: %.7f\n", min_dx);
+    PRINT_INFO("\t- min_dcost: %.7f\n", min_dcost);
+    PRINT_INFO("\t- lam_mult: %.3f\n", lam_mult);
+    PRINT_INFO("\t- min_dist: %.3f\n", min_dist);
+    PRINT_INFO("\t- max_dist: %.3f\n", max_dist);
+    PRINT_INFO("\t- max_baseline: %.3f\n", max_baseline);
+    PRINT_INFO("\t- max_cond_number: %.3f\n", max_cond_number);
   }
 };
 
diff --git a/ov_core/src/init/InertialInitializer.cpp b/ov_core/src/init/InertialInitializer.cpp
index e7e471510..7de4ed81a 100644
--- a/ov_core/src/init/InertialInitializer.cpp
+++ b/ov_core/src/init/InertialInitializer.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "InertialInitializer.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 
@@ -50,7 +51,7 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix<doubl
 
   // Return if we don't have enough for two windows
   if (newesttime - oldesttime < 2 * _window_length) {
-    // printf(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET);
+    // PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET);
     return false;
   }
 
@@ -67,7 +68,7 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix<doubl
 
   // Return if both of these failed
   if (window_1to0.empty() || window_2to1.empty()) {
-    // printf(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET);
+    // PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET);
     return false;
   }
 
@@ -97,26 +98,26 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix<doubl
     a_var_2to1 += (data.am - a_avg_2to1).dot(data.am - a_avg_2to1);
   }
   a_var_2to1 = std::sqrt(a_var_2to1 / ((int)window_2to1.size() - 1));
-  // printf(BOLDGREEN "[INIT-IMU]: IMU excitation, %.4f,%.4f\n" RESET, a_var_1to0, a_var_2to1);
+  // PRINT_DEBUG(BOLDGREEN "[INIT-IMU]: IMU excitation, %.4f,%.4f\n" RESET, a_var_1to0, a_var_2to1);
 
   // If it is below the threshold and we want to wait till we detect a jerk
   if (a_var_1to0 < _imu_excite_threshold && wait_for_jerk) {
-    printf(YELLOW "[INIT-IMU]: no IMU excitation, below threshold %.4f < %.4f\n" RESET, a_var_1to0, _imu_excite_threshold);
+    PRINT_DEBUG(YELLOW "[INIT-IMU]: no IMU excitation, below threshold %.4f < %.4f\n" RESET, a_var_1to0, _imu_excite_threshold);
     return false;
   }
 
   // We should also check that the old state was below the threshold!
   // This is the case when we have started up moving, and thus we need to wait for a period of stationary motion
   if (a_var_2to1 > _imu_excite_threshold && wait_for_jerk) {
-    printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, _imu_excite_threshold);
+    PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, _imu_excite_threshold);
     return false;
   }
 
   // If it is above the threshold and we are not waiting for a jerk
   // Then we are not stationary (i.e. moving) so we should wait till we are
   if ((a_var_1to0 > _imu_excite_threshold || a_var_2to1 > _imu_excite_threshold) && !wait_for_jerk) {
-    printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1,
-           _imu_excite_threshold);
+    PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1,
+                _imu_excite_threshold);
     return false;
   }
 
diff --git a/ov_core/src/sim/BsplineSE3.cpp b/ov_core/src/sim/BsplineSE3.cpp
index 9069bbb8d..b2978cdfe 100644
--- a/ov_core/src/sim/BsplineSE3.cpp
+++ b/ov_core/src/sim/BsplineSE3.cpp
@@ -20,6 +20,9 @@
  */
 
 #include "BsplineSE3.h"
+#include "utils/print.h"
+
+#include <sstream>
 
 using namespace ov_core;
 
@@ -32,7 +35,7 @@ void BsplineSE3::feed_trajectory(std::vector<Eigen::VectorXd> traj_points) {
   }
   dt = sumdt / (traj_points.size() - 1);
   dt = (dt < 0.05) ? 0.05 : dt;
-  printf("[B-SPLINE]: control point dt = %.3f (original dt of %.3f)\n", dt, sumdt / (traj_points.size() - 1));
+  PRINT_DEBUG("[B-SPLINE]: control point dt = %.3f (original dt of %.3f)\n", dt, sumdt / (traj_points.size() - 1));
 
   // convert all our trajectory points into SE(3) matrices
   // we are given [timestamp, p_IinG, q_GtoI]
@@ -55,8 +58,8 @@ void BsplineSE3::feed_trajectory(std::vector<Eigen::VectorXd> traj_points) {
       timestamp_max = pose.first;
     }
   }
-  printf("[B-SPLINE]: trajectory start time = %.6f\n", timestamp_min);
-  printf("[B-SPLINE]: trajectory end time = %.6f\n", timestamp_max);
+  PRINT_DEBUG("[B-SPLINE]: trajectory start time = %.6f\n", timestamp_min);
+  PRINT_DEBUG("[B-SPLINE]: trajectory end time = %.6f\n", timestamp_max);
 
   // then create spline control points
   double timestamp_curr = timestamp_min;
@@ -66,7 +69,7 @@ void BsplineSE3::feed_trajectory(std::vector<Eigen::VectorXd> traj_points) {
     double t0, t1;
     Eigen::Matrix4d pose0, pose1;
     bool success = find_bounding_poses(timestamp_curr, trajectory_points, t0, pose0, t1, pose1);
-    // printf("[SIM]: time curr = %.6f | lambda = %.3f | dt = %.3f | dtmeas =
+    // PRINT_DEBUG("[SIM]: time curr = %.6f | lambda = %.3f | dt = %.3f | dtmeas =
     // %.3f\n",timestamp_curr,(timestamp_curr-t0)/(t1-t0),dt,(t1-t0));
 
     // If we didn't find a bounding pose, then that means we are at the end of the dataset
@@ -79,12 +82,14 @@ void BsplineSE3::feed_trajectory(std::vector<Eigen::VectorXd> traj_points) {
     Eigen::Matrix4d pose_interp = exp_se3(lambda * log_se3(pose1 * Inv_se3(pose0))) * pose0;
     control_points.insert({timestamp_curr, pose_interp});
     timestamp_curr += dt;
-    // std::cout << pose_interp(0,3) << "," << pose_interp(1,3) << "," << pose_interp(2,3) << std::endl;
+    // std::stringstream ss;
+    // ss << pose_interp(0,3) << "," << pose_interp(1,3) << "," << pose_interp(2,3) << std::endl;
+    // PRINT_DEBUG(ss.str().c_str());
   }
 
   // The start time of the system is two dt in since we need at least two older control points
   timestamp_start = timestamp_min + 2 * dt;
-  printf("[B-SPLINE]: start trajectory time of %.6f\n", timestamp_start);
+  PRINT_DEBUG("[B-SPLINE]: start trajectory time of %.6f\n", timestamp_start);
 }
 
 bool BsplineSE3::get_pose(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::Vector3d &p_IinG) {
@@ -93,7 +98,7 @@ bool BsplineSE3::get_pose(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::Vect
   double t0, t1, t2, t3;
   Eigen::Matrix4d pose0, pose1, pose2, pose3;
   bool success = find_bounding_control_points(timestamp, control_points, t0, pose0, t1, pose1, t2, pose2, t3, pose3);
-  // printf("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success =
+  // PRINT_DEBUG("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success =
   // %d\n",timestamp,t0-timestamp,t1-timestamp,t2-timestamp,t3-timestamp,(int)success);
 
   // Return failure if we can't get bounding poses
@@ -129,7 +134,7 @@ bool BsplineSE3::get_velocity(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::
   double t0, t1, t2, t3;
   Eigen::Matrix4d pose0, pose1, pose2, pose3;
   bool success = find_bounding_control_points(timestamp, control_points, t0, pose0, t1, pose1, t2, pose2, t3, pose3);
-  // printf("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success =
+  // PRINT_DEBUG("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success =
   // %d\n",timestamp,t0-timestamp,t1-timestamp,t2-timestamp,t3-timestamp,(int)success);
 
   // Return failure if we can't get bounding poses
diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp
index fc3f8b26f..75ecdb83e 100644
--- a/ov_core/src/test_tracking.cpp
+++ b/ov_core/src/test_tracking.cpp
@@ -38,6 +38,7 @@
 #include "track/TrackAruco.h"
 #include "track/TrackDescriptor.h"
 #include "track/TrackKLT.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 
@@ -72,7 +73,7 @@ int main(int argc, char **argv) {
   std::string path_to_bag;
   nh.param<std::string>("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag");
   // nh.param<std::string>("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag");
-  printf("ros bag path is: %s\n", path_to_bag.c_str());
+  PRINT_INFO("ros bag path is: %s\n", path_to_bag.c_str());
 
   // Get our start location and how much of the bag we want to play
   // Make the bag duration < 0 to just process to the end of the bag
@@ -101,13 +102,13 @@ int main(int argc, char **argv) {
   nh.param<bool>("use_stereo", use_stereo, false);
 
   // Debug print!
-  printf("max features: %d\n", num_pts);
-  printf("max aruco: %d\n", num_aruco);
-  printf("clone states: %d\n", clone_states);
-  printf("grid size: %d x %d\n", grid_x, grid_y);
-  printf("fast threshold: %d\n", fast_threshold);
-  printf("min pixel distance: %d\n", min_px_dist);
-  printf("downsize aruco image: %d\n", do_downsizing);
+  PRINT_DEBUG("max features: %d\n", num_pts);
+  PRINT_DEBUG("max aruco: %d\n", num_aruco);
+  PRINT_DEBUG("clone states: %d\n", clone_states);
+  PRINT_DEBUG("grid size: %d x %d\n", grid_x, grid_y);
+  PRINT_DEBUG("fast threshold: %d\n", fast_threshold);
+  PRINT_DEBUG("min pixel distance: %d\n", min_px_dist);
+  PRINT_DEBUG("downsize aruco image: %d\n", do_downsizing);
 
   // Fake camera info (we don't need this, as we are not using the normalized coordinates for anything)
   std::unordered_map<size_t, std::shared_ptr<CamBase>> cameras;
@@ -120,7 +121,7 @@ int main(int argc, char **argv) {
   }
 
   // Lets make a feature extractor
-  //extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist);
+  // extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist);
   // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist,
   // knn_ratio);
   extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing);
@@ -144,13 +145,13 @@ int main(int argc, char **argv) {
   ros::Time time_init = view_full.getBeginTime();
   time_init += ros::Duration(bag_start);
   ros::Time time_finish = (bag_durr < 0) ? view_full.getEndTime() : time_init + ros::Duration(bag_durr);
-  printf("time start = %.6f\n", time_init.toSec());
-  printf("time end   = %.6f\n", time_finish.toSec());
+  PRINT_DEBUG("time start = %.6f\n", time_init.toSec());
+  PRINT_DEBUG("time end   = %.6f\n", time_finish.toSec());
   view.addQuery(bag, time_init, time_finish);
 
   // Check to make sure we have data to play
   if (view.size() == 0) {
-    printf(RED "No messages to play on specified topics. Exiting.\n" RESET);
+    PRINT_ERROR(RED "No messages to play on specified topics. Exiting.\n" RESET);
     ros::shutdown();
     return EXIT_FAILURE;
   }
@@ -180,7 +181,7 @@ int main(int argc, char **argv) {
       try {
         cv_ptr = cv_bridge::toCvShare(s0, sensor_msgs::image_encodings::MONO8);
       } catch (cv_bridge::Exception &e) {
-        printf(RED "cv_bridge exception: %s\n" RESET, e.what());
+        PRINT_ERROR(RED "cv_bridge exception: %s\n" RESET, e.what());
         continue;
       }
       // Save to our temp variable
@@ -198,7 +199,7 @@ int main(int argc, char **argv) {
       try {
         cv_ptr = cv_bridge::toCvShare(s1, sensor_msgs::image_encodings::MONO8);
       } catch (cv_bridge::Exception &e) {
-        printf(RED "cv_bridge exception: %s\n" RESET, e.what());
+        PRINT_ERROR(RED "cv_bridge exception: %s\n" RESET, e.what());
         continue;
       }
       // Save to our temp variable
@@ -313,7 +314,7 @@ void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1) {
     double fpf = (double)featslengths / num_lostfeats;
     double mpf = (double)num_margfeats / frames;
     // DEBUG PRINT OUT
-    printf("fps = %.2f | lost_feats/frame = %.2f | track_length/lost_feat = %.2f | marg_tracks/frame = %.2f\n", fps, lpf, fpf, mpf);
+    PRINT_DEBUG("fps = %.2f | lost_feats/frame = %.2f | track_length/lost_feat = %.2f | marg_tracks/frame = %.2f\n", fps, lpf, fpf, mpf);
     // Reset variables
     frames = 0;
     time_start = time_curr;
diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp
index 3f5032c11..00af5b0f1 100644
--- a/ov_core/src/test_webcam.cpp
+++ b/ov_core/src/test_webcam.cpp
@@ -38,6 +38,7 @@
 #include "track/TrackDescriptor.h"
 #include "track/TrackKLT.h"
 #include "utils/CLI11.hpp"
+#include "utils/print.h"
 
 using namespace ov_core;
 
@@ -82,13 +83,13 @@ int main(int argc, char **argv) {
   }
 
   // Debug print!
-  printf("max features: %d\n", num_pts);
-  printf("max aruco: %d\n", num_aruco);
-  printf("clone states: %d\n", clone_states);
-  printf("grid size: %d x %d\n", grid_x, grid_y);
-  printf("fast threshold: %d\n", fast_threshold);
-  printf("min pixel distance: %d\n", min_px_dist);
-  printf("downsize aruco image: %d\n", do_downsizing);
+  PRINT_DEBUG("max features: %d\n", num_pts);
+  PRINT_DEBUG("max aruco: %d\n", num_aruco);
+  PRINT_DEBUG("clone states: %d\n", clone_states);
+  PRINT_DEBUG("grid size: %d x %d\n", grid_x, grid_y);
+  PRINT_DEBUG("fast threshold: %d\n", fast_threshold);
+  PRINT_DEBUG("min pixel distance: %d\n", min_px_dist);
+  PRINT_DEBUG("downsize aruco image: %d\n", do_downsizing);
 
   // Fake camera info (we don't need this, as we are not using the normalized coordinates for anything)
   std::unordered_map<size_t, std::shared_ptr<CamBase>> cameras;
@@ -112,7 +113,7 @@ int main(int argc, char **argv) {
   // Open the first webcam (0=laptop cam, 1=usb device)
   cv::VideoCapture cap;
   if (!cap.open(0)) {
-    printf(RED "Unable to open a webcam feed!\n" RESET);
+    PRINT_ERROR(RED "Unable to open a webcam feed!\n" RESET);
     return EXIT_FAILURE;
   }
 
diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp
index a7285feac..4ed35ce1d 100644
--- a/ov_core/src/track/TrackAruco.cpp
+++ b/ov_core/src/track/TrackAruco.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "TrackAruco.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 
@@ -27,10 +28,10 @@ void TrackAruco::feed_new_camera(const CameraData &message) {
 
   // Error check that we have all the data
   if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) {
-    printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
-    printf(RED "[ERROR]:   - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
-    printf(RED "[ERROR]:   - message.images.size() = %zu\n" RESET, message.images.size());
-    printf(RED "[ERROR]:   - message.masks.size() = %zu\n" RESET, message.masks.size());
+    PRINT_ERROR(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
+    PRINT_ERROR(RED "[ERROR]:   - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
+    PRINT_ERROR(RED "[ERROR]:   - message.images.size() = %zu\n" RESET, message.images.size());
+    PRINT_ERROR(RED "[ERROR]:   - message.masks.size() = %zu\n" RESET, message.masks.size());
     std::exit(EXIT_FAILURE);
   }
 
@@ -44,8 +45,8 @@ void TrackAruco::feed_new_camera(const CameraData &message) {
                   }
                 }));
 #else
-    printf(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
-    std::exit(EXIT_FAILURE);
+  PRINT_ERROR(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
+  std::exit(EXIT_FAILURE);
 #endif
 }
 
@@ -147,9 +148,9 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t
   rT3 = boost::posix_time::microsec_clock::local_time();
 
   // Timing information
-  // printf("[TIME-ARUCO]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[TIME-ARUCO]: %.4f seconds for feature DB update (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6,
-  // (int)good_left.size()); printf("[TIME-ARUCO]: %.4f seconds for total\n",(rT3-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for feature DB update (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6,
+  // (int)good_left.size()); PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for total\n",(rT3-rT1).total_microseconds() * 1e-6);
 }
 
 void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2) {
diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h
index fd99430c7..431d4c9e0 100644
--- a/ov_core/src/track/TrackAruco.h
+++ b/ov_core/src/track/TrackAruco.h
@@ -27,6 +27,7 @@
 #endif
 
 #include "TrackBase.h"
+#include "utils/print.h"
 
 namespace ov_core {
 
@@ -57,7 +58,7 @@ class TrackAruco : public TrackBase {
     // NOTE: people with newer opencv might fail here
     // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX;
 #else
-    printf(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
+    PRINT_ERROR(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET);
     std::exit(EXIT_FAILURE);
 #endif
   }
@@ -107,7 +108,6 @@ class TrackAruco : public TrackBase {
   std::unordered_map<size_t, std::vector<int>> ids_aruco;
   std::unordered_map<size_t, std::vector<std::vector<cv::Point2f>>> corners, rejects;
 #endif
-
 };
 
 } // namespace ov_core
diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp
index 890faa23f..6857dbf6e 100644
--- a/ov_core/src/track/TrackDescriptor.cpp
+++ b/ov_core/src/track/TrackDescriptor.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "TrackDescriptor.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 
@@ -27,10 +28,10 @@ void TrackDescriptor::feed_new_camera(const CameraData &message) {
 
   // Error check that we have all the data
   if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) {
-    printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
-    printf(RED "[ERROR]:   - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
-    printf(RED "[ERROR]:   - message.images.size() = %zu\n" RESET, message.images.size());
-    printf(RED "[ERROR]:   - message.masks.size() = %zu\n" RESET, message.masks.size());
+    PRINT_ERROR(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
+    PRINT_ERROR(RED "[ERROR]:   - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
+    PRINT_ERROR(RED "[ERROR]:   - message.images.size() = %zu\n" RESET, message.images.size());
+    PRINT_ERROR(RED "[ERROR]:   - message.masks.size() = %zu\n" RESET, message.masks.size());
     std::exit(EXIT_FAILURE);
   }
 
@@ -48,7 +49,7 @@ void TrackDescriptor::feed_new_camera(const CameraData &message) {
                     }
                   }));
   } else {
-    printf(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images);
+    PRINT_ERROR(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images);
     std::exit(EXIT_FAILURE);
   }
 }
@@ -141,7 +142,7 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) {
   }
 
   // Debug info
-  // printf("LtoL = %d | good = %d | fromlast = %d\n",(int)matches_ll.size(),(int)good_left.size(),num_tracklast);
+  // PRINT_DEBUG("LtoL = %d | good = %d | fromlast = %d\n",(int)matches_ll.size(),(int)good_left.size(),num_tracklast);
 
   // Move forward in time
   img_last[cam_id] = img;
@@ -152,11 +153,11 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) {
   rT5 = boost::posix_time::microsec_clock::local_time();
 
   // Our timing information
-  // printf("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6);
-  // printf("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6);
-  // printf("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)good_left.size());
-  // printf("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6,
+  // (int)good_left.size()); PRINT_DEBUG("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
 }
 
 void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) {
@@ -297,7 +298,7 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left,
   }
 
   // Debug info
-  // printf("LtoL = %d | RtoR = %d | LtoR = %d | good = %d | fromlast = %d\n", (int)matches_ll.size(),
+  // PRINT_DEBUG("LtoL = %d | RtoR = %d | LtoR = %d | good = %d | fromlast = %d\n", (int)matches_ll.size(),
   //       (int)matches_rr.size(),(int)ids_left_new.size(),(int)good_left.size(),num_tracklast);
 
   // Move forward in time
@@ -314,11 +315,11 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left,
   rT5 = boost::posix_time::microsec_clock::local_time();
 
   // Our timing information
-  // printf("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6);
-  // printf("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6);
-  // printf("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)good_left.size());
-  // printf("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6,
+  // (int)good_left.size()); PRINT_DEBUG("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
 }
 
 void TrackDescriptor::perform_detection_monocular(const cv::Mat &img0, const cv::Mat &mask0, std::vector<cv::KeyPoint> &pts0,
diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp
index a262015c9..80c3d0b40 100644
--- a/ov_core/src/track/TrackKLT.cpp
+++ b/ov_core/src/track/TrackKLT.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "TrackKLT.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 
@@ -27,10 +28,10 @@ void TrackKLT::feed_new_camera(const CameraData &message) {
 
   // Error check that we have all the data
   if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) {
-    printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
-    printf(RED "[ERROR]:   - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
-    printf(RED "[ERROR]:   - message.images.size() = %zu\n" RESET, message.images.size());
-    printf(RED "[ERROR]:   - message.masks.size() = %zu\n" RESET, message.masks.size());
+    PRINT_ERROR(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET);
+    PRINT_ERROR(RED "[ERROR]:   - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size());
+    PRINT_ERROR(RED "[ERROR]:   - message.images.size() = %zu\n" RESET, message.images.size());
+    PRINT_ERROR(RED "[ERROR]:   - message.masks.size() = %zu\n" RESET, message.masks.size());
     std::exit(EXIT_FAILURE);
   }
 
@@ -48,7 +49,7 @@ void TrackKLT::feed_new_camera(const CameraData &message) {
                     }
                   }));
   } else {
-    printf(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images);
+    PRINT_ERROR(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images);
     std::exit(EXIT_FAILURE);
   }
 }
@@ -114,7 +115,7 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) {
     img_mask_last[cam_id] = mask;
     pts_last[cam_id].clear();
     ids_last[cam_id].clear();
-    printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET);
+    PRINT_ERROR(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET);
     return;
   }
 
@@ -154,11 +155,11 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) {
   rT5 = boost::posix_time::microsec_clock::local_time();
 
   // Timing information
-  // printf("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)good_left.size());
-  // printf("[TIME-KLT]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6,
+  // (int)good_left.size()); PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
 }
 
 void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) {
@@ -265,7 +266,7 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t
     pts_last[cam_id_right].clear();
     ids_last[cam_id_left].clear();
     ids_last[cam_id_right].clear();
-    printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET);
+    PRINT_ERROR(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET);
     return;
   }
 
@@ -300,11 +301,11 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t
       good_right.push_back(pts_right_new.at(index_right));
       good_ids_left.push_back(ids_last[cam_id_left].at(i));
       good_ids_right.push_back(ids_last[cam_id_right].at(index_right));
-      // std::cout << "adding to stereo - " << ids_last[cam_id_left].at(i) << " , " << ids_last[cam_id_right].at(index_right) << std::endl;
+      // PRINT_DEBUG("adding to stereo - %u , %u\n", ids_last[cam_id_left].at(i), ids_last[cam_id_right].at(index_right));
     } else if (mask_ll[i]) {
       good_left.push_back(pts_left_new.at(i));
       good_ids_left.push_back(ids_last[cam_id_left].at(i));
-      // std::cout << "adding to left - " << ids_last[cam_id_left].at(i) << std::endl;
+      // PRINT_DEBUG("adding to left - %u \n", ids_last[cam_id_left].at(i));
     }
   }
 
@@ -320,7 +321,7 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t
     if (mask_rr[i] && !added_already) {
       good_right.push_back(pts_right_new.at(i));
       good_ids_right.push_back(ids_last[cam_id_right].at(i));
-      // std::cout << "adding to right - " << ids_last[cam_id_right].at(i) << std::endl;
+      // PRINT_DEBUG("adding to right - %u \n", ids_last[cam_id_right].at(i));
     }
   }
 
@@ -350,12 +351,12 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t
   rT6 = boost::posix_time::microsec_clock::local_time();
 
   // Timing information
-  // printf("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for stereo klt\n",(rT5-rT4).total_microseconds() * 1e-6);
-  // printf("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT6-rT5).total_microseconds() * 1e-6, (int)good_left.size());
-  // printf("[TIME-KLT]: %.4f seconds for total\n",(rT6-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for stereo klt\n",(rT5-rT4).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT6-rT5).total_microseconds() * 1e-6,
+  // (int)good_left.size()); PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n",(rT6-rT1).total_microseconds() * 1e-6);
 }
 
 void TrackKLT::perform_detection_monocular(const std::vector<cv::Mat> &img0pyr, const cv::Mat &mask0, std::vector<cv::KeyPoint> &pts0,
diff --git a/ov_core/src/track/TrackSIM.h b/ov_core/src/track/TrackSIM.h
index 9ddd61e06..6fcd4dfb5 100644
--- a/ov_core/src/track/TrackSIM.h
+++ b/ov_core/src/track/TrackSIM.h
@@ -23,6 +23,7 @@
 #define OV_CORE_TRACK_SIM_H
 
 #include "TrackBase.h"
+#include "utils/print.h"
 
 namespace ov_core {
 
@@ -54,8 +55,8 @@ class TrackSIM : public TrackBase {
    * @param message Contains our timestamp, images, and camera ids
    */
   void feed_new_camera(const CameraData &message) override {
-    printf(RED "[SIM]: SIM TRACKER FEED NEW CAMERA CALLED!!!\n" RESET);
-    printf(RED "[SIM]: THIS SHOULD NEVER HAPPEN!\n" RESET);
+    PRINT_ERROR(RED "[SIM]: SIM TRACKER FEED NEW CAMERA CALLED!!!\n" RESET);
+    PRINT_ERROR(RED "[SIM]: THIS SHOULD NEVER HAPPEN!\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
diff --git a/ov_core/src/types/Landmark.h b/ov_core/src/types/Landmark.h
index 2f9aa963d..c6fff6d44 100644
--- a/ov_core/src/types/Landmark.h
+++ b/ov_core/src/types/Landmark.h
@@ -25,6 +25,7 @@
 #include "LandmarkRepresentation.h"
 #include "Vec.h"
 #include "utils/colors.h"
+#include "utils/print.h"
 
 namespace ov_type {
 
@@ -79,7 +80,7 @@ class Landmark : public Vec {
     set_value(_value + dx);
     // Ensure we are not near zero in the z-direction
     if (LandmarkRepresentation::is_relative_representation(_feat_representation) && _value(_value.rows() - 1) < 1e-8) {
-      printf(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows() - 1));
+      PRINT_WARNING(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows() - 1));
       should_marg = true;
     }
   }
diff --git a/ov_core/src/utils/dataset_reader.h b/ov_core/src/utils/dataset_reader.h
index 9e3da37a1..49b2bd749 100644
--- a/ov_core/src/utils/dataset_reader.h
+++ b/ov_core/src/utils/dataset_reader.h
@@ -29,6 +29,7 @@
 #include <string>
 
 #include "colors.h"
+#include "print.h"
 
 using namespace std;
 
@@ -70,8 +71,8 @@ class DatasetReader {
 
     // Check that it was successfull
     if (!file) {
-      printf(RED "ERROR: Unable to open groundtruth file...\n" RESET);
-      printf(RED "ERROR: %s\n" RESET, path.c_str());
+      PRINT_ERROR(RED "ERROR: Unable to open groundtruth file...\n" RESET);
+      PRINT_ERROR(RED "ERROR: %s\n" RESET, path.c_str());
       std::exit(EXIT_FAILURE);
     }
 
@@ -89,8 +90,8 @@ class DatasetReader {
       while (getline(s, field, ',')) {
         // Ensure we are in the range
         if (i > 16) {
-          printf(RED "ERROR: Invalid groudtruth line, too long!\n" RESET);
-          printf(RED "ERROR: %s\n" RESET, line.c_str());
+          PRINT_ERROR(RED "ERROR: Invalid groudtruth line, too long!\n" RESET);
+          PRINT_ERROR(RED "ERROR: %s\n" RESET, line.c_str());
           std::exit(EXIT_FAILURE);
         }
         // Save our groundtruth state value
@@ -115,7 +116,7 @@ class DatasetReader {
 
     // Check that we even have groundtruth loaded
     if (gt_states.empty()) {
-      printf(RED "Groundtruth data loaded is empty, make sure you call load before asking for a state.\n" RESET);
+      PRINT_ERROR(RED "Groundtruth data loaded is empty, make sure you call load before asking for a state.\n" RESET);
       return false;
     }
 
@@ -131,14 +132,14 @@ class DatasetReader {
 
     // If close to this timestamp, then use it
     if (std::abs(closest_time - timestep) < 0.10) {
-      // printf("init DT = %.4f\n", std::abs(closest_time-timestep));
-      // printf("timestamp = %.15f\n", closest_time);
+      // PRINT_DEBUG("init DT = %.4f\n", std::abs(closest_time-timestep));
+      // PRINT_DEBUG("timestamp = %.15f\n", closest_time);
       timestep = closest_time;
     }
 
     // Check that we have the timestamp in our GT file
     if (gt_states.find(timestep) == gt_states.end()) {
-      printf(YELLOW "Unable to find %.6f timestamp in GT file, wrong GT file loaded???\n" RESET, timestep);
+      PRINT_INFO(YELLOW "Unable to find %.6f timestamp in GT file, wrong GT file loaded???\n" RESET, timestep);
       return false;
     }
 
diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp
new file mode 100644
index 000000000..39592bbed
--- /dev/null
+++ b/ov_core/src/utils/print.cpp
@@ -0,0 +1,84 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2021 Patrick Geneva
+ * Copyright (C) 2021 Guoquan Huang
+ * Copyright (C) 2021 OpenVINS Contributors
+ * Copyright (C) 2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "print.h"
+#include <cstdarg>
+#include <iostream>
+#include <stdio.h>
+#include <string.h>
+
+using namespace ov_core;
+
+// Need to define the variable for everything to work
+Printer::PrintLevel Printer::current_print_level = PrintLevel::ALL;
+
+void Printer::setPrintLevel(PrintLevel level) {
+  Printer::current_print_level = level;
+
+  std::cout << "Setting printing level to: ";
+  switch (current_print_level) {
+  case PrintLevel::ALL:
+    std::cout << "ALL";
+    break;
+  case PrintLevel::DEBUG:
+    std::cout << "DEBUG";
+    break;
+  case PrintLevel::INFO:
+    std::cout << "INFO";
+    break;
+  case PrintLevel::WARNING:
+    std::cout << "WARNING";
+    break;
+  case PrintLevel::ERROR:
+    std::cout << "ERROR";
+    break;
+  case PrintLevel::SILENT:
+    std::cout << "SILENT";
+    break;
+  default:
+    // Can never get here
+    break;
+  }
+
+  std::cout << std::endl;
+}
+
+void Printer::debugPrint(PrintLevel level, const char location[], const char *format, ...) {
+  // Only print for the current debug level
+  if (static_cast<int>(level) < static_cast<int>(Printer::current_print_level)) {
+    return;
+  }
+
+  // Print the location info first
+  if (strlen(location) > MAX_FILE_PATH_LEGTH) {
+    // Truncate the location length to the max size for the filepath
+    printf("%s", &(location[strlen(location) - MAX_FILE_PATH_LEGTH]));
+  } else {
+    // Print the full location
+    printf("%s", location);
+  }
+
+  // Print the rest of the args
+  va_list args;
+  va_start(args, format);
+  vprintf(format, args);
+  va_end(args);
+}
diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h
new file mode 100644
index 000000000..c0bfa267c
--- /dev/null
+++ b/ov_core/src/utils/print.h
@@ -0,0 +1,81 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2021 Patrick Geneva
+ * Copyright (C) 2021 Guoquan Huang
+ * Copyright (C) 2021 OpenVINS Contributors
+ * Copyright (C) 2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef PRINT_H
+#define PRINT_H
+
+#include <stdint.h>
+
+namespace ov_core {
+
+/**
+ * @brief Printer for open_vins that allows for various levels of printing to be done
+ *
+ */
+class Printer {
+public:
+  /** The different print levels possible
+   */
+  enum PrintLevel { ALL = 0, DEBUG = 1, INFO = 2, WARNING = 3, ERROR = 4, SILENT = 5 };
+
+  /** @brief Set the print level to use for all future printing to stdout.
+   *
+   *  @param level The debug level to use
+   */
+  static void setPrintLevel(PrintLevel level);
+
+  /** brief The print function that prints to stdout.
+   *
+   *  @param level the print level for this print call
+   *  @param location the location the print was made from
+   *  @param format The printf format
+   */
+  static void debugPrint(PrintLevel level, const char location[], const char *format, ...);
+
+  /** brief The print level
+   */
+  static PrintLevel current_print_level;
+
+private:
+  /// The max length for the file path.  This is to avoid very long file paths from
+  static constexpr uint32_t MAX_FILE_PATH_LEGTH = 50;
+};
+
+} /* namespace ov_core */
+
+/** Converts anything to a string
+ */
+#define STRINGIFY(x) #x
+#define TOSTRING(x) STRINGIFY(x)
+
+/** Adds line numbers
+ */
+#define AT "(" __FILE__ ":" TOSTRING(__LINE__) "): "
+
+/** The different Types of print levels
+ */
+#define PRINT_ALL(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ALL, AT, x);
+#define PRINT_DEBUG(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::DEBUG, AT, x);
+#define PRINT_INFO(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::INFO, AT, x);
+#define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, AT, x);
+#define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, AT, x);
+
+#endif /* PRINT_H */
diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h
index a8989e22c..adfadc588 100644
--- a/ov_core/src/utils/quat_ops.h
+++ b/ov_core/src/utils/quat_ops.h
@@ -61,6 +61,8 @@
  *
  */
 
+#include "utils/print.h"
+
 #include <Eigen/Eigen>
 #include <iostream>
 #include <sstream>
@@ -91,26 +93,26 @@ inline Eigen::Matrix<double, 4, 1> rot_2_quat(const Eigen::Matrix<double, 3, 3>
   Eigen::Matrix<double, 4, 1> q;
   double T = rot.trace();
   if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) {
-    // cout << "case 1- " << endl;
+    // PRINT_DEBUG("case 1- \n");
     q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4);
     q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0));
     q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0));
     q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1));
 
   } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) {
-    // cout << "case 2- " << endl;
+    // PRINT_DEBUG("case 2- \n");
     q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4);
     q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0));
     q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1));
     q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2));
   } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) {
-    // cout << "case 3- " << endl;
+    // PRINT_DEBUG("case 3- \n");
     q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4);
     q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0));
     q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1));
     q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0));
   } else {
-    // cout << "case 4- " << endl;
+    // PRINT_DEBUG("case 4- \n");
     q(3) = sqrt((1 + T) / 4);
     q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1));
     q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2));
diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt
index 3ceae0906..ac3a547ae 100644
--- a/ov_eval/CMakeLists.txt
+++ b/ov_eval/CMakeLists.txt
@@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.8.8)
 project(ov_eval)
 
 # Find catkin (the ROS build system)
-find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs)
+find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core)
 
 # Include libraries
 find_package(Eigen3 REQUIRED)
@@ -29,9 +29,9 @@ option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)"
 if (catkin_FOUND AND ENABLE_CATKIN_ROS)
     add_definitions(-DROS_AVAILABLE=1)
     catkin_package(
-            CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs
+            CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core
             INCLUDE_DIRS src
-            LIBRARIES ov_core_lib
+            LIBRARIES ov_eval_lib
     )
 else()
     add_definitions(-DROS_AVAILABLE=0)
diff --git a/ov_eval/package.xml b/ov_eval/package.xml
index 2af2b4dc1..ebcb85ae9 100644
--- a/ov_eval/package.xml
+++ b/ov_eval/package.xml
@@ -26,6 +26,7 @@
     <build_depend>geometry_msgs</build_depend>
     <build_depend>nav_msgs</build_depend>
     <build_depend>sensor_msgs</build_depend>
+    <build_depend>ov_core</build_depend>
 
     <!-- Dependencies needed after this package is compiled. -->
     <run_depend>roscpp</run_depend>
@@ -33,6 +34,7 @@
     <run_depend>geometry_msgs</run_depend>
     <run_depend>nav_msgs</run_depend>
     <run_depend>sensor_msgs</run_depend>
+    <run_depend>ov_core</run_depend>
 
 
 </package>
\ No newline at end of file
diff --git a/ov_eval/src/alignment/AlignTrajectory.cpp b/ov_eval/src/alignment/AlignTrajectory.cpp
index 99c8d8126..fa4f75b48 100644
--- a/ov_eval/src/alignment/AlignTrajectory.cpp
+++ b/ov_eval/src/alignment/AlignTrajectory.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "AlignTrajectory.h"
+#include "utils/print.h"
 
 using namespace ov_eval;
 
@@ -48,8 +49,8 @@ void AlignTrajectory::align_trajectory(const std::vector<Eigen::Matrix<double, 7
     R.setIdentity();
     t.setZero();
   } else {
-    printf(RED "[ALIGN]: Invalid alignment method!\n" RESET);
-    printf(RED "[ALIGN]: Possible options: posyaw, posyawsingle, se3, se3single, sim3, none\n" RESET);
+    PRINT_ERROR(RED "[ALIGN]: Invalid alignment method!\n" RESET);
+    PRINT_ERROR(RED "[ALIGN]: Possible options: posyaw, posyawsingle, se3, se3single, sim3, none\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 }
diff --git a/ov_eval/src/alignment/AlignUtils.cpp b/ov_eval/src/alignment/AlignUtils.cpp
index d12ac8ff1..4341cd159 100644
--- a/ov_eval/src/alignment/AlignUtils.cpp
+++ b/ov_eval/src/alignment/AlignUtils.cpp
@@ -20,6 +20,9 @@
  */
 
 #include "AlignUtils.h"
+#include "utils/print.h"
+
+#include <sstream>
 
 using namespace ov_eval;
 
@@ -86,8 +89,10 @@ void AlignUtils::align_umeyama(const std::vector<Eigen::Matrix<double, 3, 1>> &d
   t.noalias() = mu_M - s * R * mu_D;
 
   // Debug printing
-  // std::cout << "R- " << std::endl << R << std::endl;
-  // std::cout << "t- " << std::endl << t << std::endl;
+  // std::stringstream ss;
+  // ss << "R- " << std::endl << R << std::endl;
+  // ss << "t- " << std::endl << t << std::endl;
+  // PRINT_DEBUG(ss.str().c_str());
 }
 
 void AlignUtils::perform_association(double offset, double max_difference, std::vector<double> &est_times, std::vector<double> &gt_times,
@@ -166,13 +171,14 @@ void AlignUtils::perform_association(double offset, double max_difference, std::
 
   // Ensure that we have enough associations
   if (est_times.size() < 3) {
-    printf(RED "[ALIGN]: Was unable to associate groundtruth and estimate trajectories\n" RESET);
-    printf(RED "[ALIGN]: %d total matches....\n" RESET, (int)est_times.size());
-    printf(RED "[ALIGN]: Do the time of the files match??\n" RESET);
+    PRINT_ERROR(RED "[ALIGN]: Was unable to associate groundtruth and estimate trajectories\n" RESET);
+    PRINT_ERROR(RED "[ALIGN]: %d total matches....\n" RESET, (int)est_times.size());
+    PRINT_ERROR(RED "[ALIGN]: Do the time of the files match??\n" RESET);
     return;
   }
   assert(est_times_temp.size() == gt_times_temp.size());
-  // printf("[TRAJ]: %d est poses and %d gt poses => %d matches\n",(int)est_times.size(),(int)gt_times.size(),(int)est_times_temp.size());
+  // PRINT_DEBUG("[TRAJ]: %d est poses and %d gt poses => %d
+  // matches\n",(int)est_times.size(),(int)gt_times.size(),(int)est_times_temp.size());
 
   // Overwrite with intersected values
   est_times = est_times_temp;
diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp
index 4d5874af4..f94502659 100644
--- a/ov_eval/src/calc/ResultSimulation.cpp
+++ b/ov_eval/src/calc/ResultSimulation.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "ResultSimulation.h"
+#include "utils/print.h"
 
 using namespace ov_eval;
 
@@ -35,8 +36,8 @@ ResultSimulation::ResultSimulation(std::string path_est, std::string path_std, s
   assert(est_state.size() == gt_state.size());
 
   // Debug print
-  printf("[SIM]: loaded %d timestamps from file!!\n", (int)est_state.size());
-  printf("[SIM]: we have %d cameras in total!!\n", (int)est_state.at(0)(18));
+  PRINT_DEBUG("[SIM]: loaded %d timestamps from file!!\n", (int)est_state.size());
+  PRINT_DEBUG("[SIM]: we have %d cameras in total!!\n", (int)est_state.at(0)(18));
 }
 
 void ResultSimulation::plot_state(bool doplotting, double max_time) {
@@ -213,7 +214,7 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) {
 
     // If we are not calibrating then don't plot it!
     if (state_cov.at(i)(16) == 0.0) {
-      printf(YELLOW "Time offset was not calibrated online, so will not plot...\n" RESET);
+      PRINT_WARNING(YELLOW "Time offset was not calibrated online, so will not plot...\n" RESET);
       return;
     }
 
@@ -229,7 +230,7 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) {
     return;
 
 #ifndef HAVE_PYTHONLIBS
-  printf(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
+  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
   return;
 #endif
 #ifdef HAVE_PYTHONLIBS
@@ -279,7 +280,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) {
 
   // Check that we have cameras
   if ((int)est_state.at(0)(18) < 1) {
-    printf(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
+    PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
     return;
   }
 
@@ -308,7 +309,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) {
 
     // If we are not calibrating then don't plot it!
     if (state_cov.at(i)(18) == 0.0) {
-      printf(YELLOW "Camera intrinsics not calibrated online, so will not plot...\n" RESET);
+      PRINT_WARNING(YELLOW "Camera intrinsics not calibrated online, so will not plot...\n" RESET);
       return;
     }
 
@@ -330,7 +331,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) {
     return;
 
 #ifndef HAVE_PYTHONLIBS
-  printf(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
+  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
   return;
 #endif
 #ifdef HAVE_PYTHONLIBS
@@ -392,7 +393,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) {
 
   // Check that we have cameras
   if ((int)est_state.at(0)(18) < 1) {
-    printf(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
+    PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
     return;
   }
 
@@ -421,7 +422,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) {
 
     // If we are not calibrating then don't plot it!
     if (state_cov.at(i)(26) == 0.0) {
-      printf(YELLOW "Camera extrinsics not calibrated online, so will not plot...\n" RESET);
+      PRINT_WARNING(YELLOW "Camera extrinsics not calibrated online, so will not plot...\n" RESET);
       return;
     }
 
@@ -449,7 +450,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) {
     return;
 
 #ifndef HAVE_PYTHONLIBS
-  printf(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
+  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
   return;
 #endif
 #ifdef HAVE_PYTHONLIBS
diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp
index e0bb7b375..e5d83c127 100644
--- a/ov_eval/src/calc/ResultTrajectory.cpp
+++ b/ov_eval/src/calc/ResultTrajectory.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "ResultTrajectory.h"
+#include "utils/print.h"
 
 using namespace ov_eval;
 
@@ -32,16 +33,16 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st
   // Debug print amount
   // std::string base_filename1 = path_est.substr(path_est.find_last_of("/\\") + 1);
   // std::string base_filename2 = path_gt.substr(path_gt.find_last_of("/\\") + 1);
-  // printf("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str());
-  // printf("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str());
+  // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str());
+  // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str());
 
   // Intersect timestamps
   AlignUtils::perform_association(0, 0.02, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori, gt_covpos);
 
   // Return failure if we didn't have any common timestamps
   if (est_poses.size() < 3) {
-    printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
-    printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
+    PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
+    PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -55,9 +56,9 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st
   // Debug print to the user
   Eigen::Vector4d q_ESTtoGT = Math::rot_2_quat(R_ESTtoGT);
   Eigen::Vector4d q_GTtoEST = Math::rot_2_quat(R_GTtoEST);
-  printf("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2),
-         q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
-  // printf("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s =
+  PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
+              q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
+  // PRINT_DEBUG("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s =
   // %.2f\n",q_GTtoEST(0),q_GTtoEST(1),q_GTtoEST(2),q_GTtoEST(3),t_GTinEST(0),t_GTinEST(1),t_GTinEST(2),s_GTtoEST);
 
   // Finally lets calculate the aligned trajectories
@@ -147,10 +148,11 @@ void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths,
   // Warn if large pos difference
   double max_dist_diff = 0.5;
   if (average_pos_difference > max_dist_diff) {
-    printf(YELLOW "[COMP]: average groundtruth position difference %.2f > %.2f is too large\n" RESET, average_pos_difference,
-           max_dist_diff);
-    printf(YELLOW "[COMP]: this will prevent the RPE from finding valid trajectory segments!!!\n" RESET);
-    printf(YELLOW "[COMP]: the recommendation is to use a higher frequency groundtruth, or relax this trajectory segment logic...\n" RESET);
+    PRINT_WARNING(YELLOW "[COMP]: average groundtruth position difference %.2f > %.2f is too large\n" RESET, average_pos_difference,
+                  max_dist_diff);
+    PRINT_WARNING(YELLOW "[COMP]: this will prevent the RPE from finding valid trajectory segments!!!\n" RESET);
+    PRINT_WARNING(YELLOW
+                  "[COMP]: the recommendation is to use a higher frequency groundtruth, or relax this trajectory segment logic...\n" RESET);
   }
 
   // Loop through each segment length
@@ -238,8 +240,8 @@ void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos
   // Check that we have our covariance matrices to normalize with
   if (est_times.size() != est_covori.size() || est_times.size() != est_covpos.size() || gt_times.size() != gt_covori.size() ||
       gt_times.size() != gt_covpos.size()) {
-    printf(YELLOW "[TRAJ]: Normalized Estimation Error Squared called but trajectory does not have any covariances...\n" RESET);
-    printf(YELLOW "[TRAJ]: Did you record using a Odometry or PoseWithCovarianceStamped????\n" RESET);
+    PRINT_WARNING(YELLOW "[TRAJ]: Normalized Estimation Error Squared called but trajectory does not have any covariances...\n" RESET);
+    PRINT_WARNING(YELLOW "[TRAJ]: Did you record using a Odometry or PoseWithCovarianceStamped????\n" RESET);
     return;
   }
 
@@ -265,7 +267,7 @@ void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos
 
     // Skip if nan error value
     if (std::isnan(ori_nees) || std::isnan(pos_nees)) {
-      printf(YELLOW "[TRAJ]: nees calculation had nan number (covariance is wrong?) skipping...\n" RESET);
+      PRINT_WARNING(YELLOW "[TRAJ]: nees calculation had nan number (covariance is wrong?) skipping...\n" RESET);
       continue;
     }
 
diff --git a/ov_eval/src/error_comparison.cpp b/ov_eval/src/error_comparison.cpp
index e4656480f..cc7596fdf 100644
--- a/ov_eval/src/error_comparison.cpp
+++ b/ov_eval/src/error_comparison.cpp
@@ -28,6 +28,7 @@
 #include "calc/ResultTrajectory.h"
 #include "utils/Colors.h"
 #include "utils/Loader.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -42,9 +43,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 4) {
-    printf(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET);
-    printf(RED "ERROR: ./error_comparison <align_mode> <folder_groundtruth> <folder_algorithms>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval error_comparison <align_mode> <folder_groundtruth> <folder_algorithms>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_comparison <align_mode> <folder_groundtruth> <folder_algorithms>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_comparison <align_mode> <folder_groundtruth> <folder_algorithms>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -67,7 +68,7 @@ int main(int argc, char **argv) {
     ov_eval::Loader::load_data(path_groundtruths.at(i).string(), times, poses, cov_ori, cov_pos);
     // Print its length and stats
     double length = ov_eval::Loader::get_total_length(poses);
-    printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_groundtruths.at(i).filename().c_str(), length);
+    PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_groundtruths.at(i).filename().c_str(), length);
   }
 
   // Get the algorithms we will process
@@ -120,8 +121,8 @@ int main(int argc, char **argv) {
   for (size_t i = 0; i < path_algorithms.size(); i++) {
 
     // Debug print
-    printf("======================================\n");
-    printf("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str());
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str());
 
     // Get the list of datasets this algorithm records
     std::map<std::string, boost::filesystem::path> path_algo_datasets;
@@ -136,14 +137,14 @@ int main(int argc, char **argv) {
 
       // Check if we have runs for this dataset
       if (path_algo_datasets.find(path_groundtruths.at(j).stem().string()) == path_algo_datasets.end()) {
-        printf(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(),
-               path_groundtruths.at(j).stem().c_str());
+        PRINT_ERROR(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(),
+                    path_groundtruths.at(j).stem().c_str());
         continue;
       }
 
       // Debug print
-      printf("[COMP]: processing %s algorithm => %s dataset\n", path_algorithms.at(i).filename().c_str(),
-             path_groundtruths.at(j).stem().c_str());
+      PRINT_DEBUG("[COMP]: processing %s algorithm => %s dataset\n", path_algorithms.at(i).filename().c_str(),
+                  path_groundtruths.at(j).stem().c_str());
 
       // Errors for this specific dataset (i.e. our averages over the total runs)
       ov_eval::Statistics ate_dataset_ori;
@@ -198,17 +199,17 @@ int main(int argc, char **argv) {
 
       // Print stats for this specific dataset
       std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : "";
-      printf("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean,
-             (int)ate_dataset_pos.values.size());
-      printf("\tATE: std_ori  = %.3f | std_pos  = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std);
+      PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean,
+                  ate_dataset_pos.mean, (int)ate_dataset_pos.values.size());
+      PRINT_DEBUG("\tATE: std_ori  = %.3f | std_pos  = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std);
       for (auto &seg : rpe_dataset) {
         seg.second.first.calculate();
         seg.second.second.calculate();
-        // printf("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d
+        // PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d
         // samples)\n",(int)seg.first,seg.second.first.mean,seg.second.second.mean,(int)seg.second.second.values.size());
-        printf("\tRPE: seg %d - median_ori = %.4f | median_pos = %.4f (%d samples)\n", (int)seg.first, seg.second.first.median,
-               seg.second.second.median, (int)seg.second.second.values.size());
-        // printf("RPE: seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
+        PRINT_DEBUG("\tRPE: seg %d - median_ori = %.4f | median_pos = %.4f (%d samples)\n", (int)seg.first, seg.second.first.median,
+                    seg.second.second.median, (int)seg.second.second.values.size());
+        // PRINT_DEBUG("RPE: seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
       }
 
       // Update the global ATE error stats
@@ -235,56 +236,56 @@ int main(int argc, char **argv) {
   //===============================================================================
 
   // Finally print the ATE for all the runs
-  printf("============================================\n");
-  printf("ATE LATEX TABLE\n");
-  printf("============================================\n");
+  PRINT_DEBUG("============================================\n");
+  PRINT_DEBUG("ATE LATEX TABLE\n");
+  PRINT_DEBUG("============================================\n");
   for (size_t i = 0; i < path_groundtruths.size(); i++) {
     std::string gtname = path_groundtruths.at(i).stem().string();
     boost::replace_all(gtname, "_", "\\_");
-    printf(" & \\textbf{%s}", gtname.c_str());
+    PRINT_DEBUG(" & \\textbf{%s}", gtname.c_str());
   }
-  printf(" & \\textbf{Average} \\\\\\hline\n");
+  PRINT_DEBUG(" & \\textbf{Average} \\\\\\hline\n");
   for (auto &algo : algo_ate) {
     std::string algoname = algo.first;
     boost::replace_all(algoname, "_", "\\_");
-    cout << algoname;
+    PRINT_DEBUG(algoname.c_str());
     double sum_ori = 0.0;
     double sum_pos = 0.0;
     int sum_ct = 0;
     for (auto &seg : algo.second) {
       if (seg.first.values.empty() || seg.second.values.empty()) {
-        printf(" & - / -");
+        PRINT_DEBUG(" & - / -");
       } else {
-        printf(" & %.3f / %.3f", seg.first.rmse, seg.second.rmse);
+        PRINT_DEBUG(" & %.3f / %.3f", seg.first.rmse, seg.second.rmse);
         sum_ori += seg.first.rmse;
         sum_pos += seg.second.rmse;
         sum_ct++;
       }
     }
-    printf(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct);
+    PRINT_DEBUG(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct);
   }
-  printf("============================================\n");
+  PRINT_DEBUG("============================================\n");
 
   // Finally print the RPE for all the runs
-  printf("============================================\n");
-  printf("RPE LATEX TABLE\n");
-  printf("============================================\n");
+  PRINT_DEBUG("============================================\n");
+  PRINT_DEBUG("RPE LATEX TABLE\n");
+  PRINT_DEBUG("============================================\n");
   for (const auto &len : segments) {
-    printf(" & \\textbf{%dm}", (int)len);
+    PRINT_DEBUG(" & \\textbf{%dm}", (int)len);
   }
-  printf(" \\\\\\hline\n");
+  PRINT_DEBUG(" \\\\\\hline\n");
   for (auto &algo : algo_rpe) {
     std::string algoname = algo.first;
     boost::replace_all(algoname, "_", "\\_");
-    cout << algoname;
+    PRINT_DEBUG(algoname.c_str());
     for (auto &seg : algo.second) {
       seg.second.first.calculate();
       seg.second.second.calculate();
-      printf(" & %.3f / %.3f", seg.second.first.median, seg.second.second.median);
+      PRINT_DEBUG(" & %.3f / %.3f", seg.second.first.median, seg.second.second.median);
     }
-    printf(" \\\\\n");
+    PRINT_DEBUG(" \\\\\n");
   }
-  printf("============================================\n");
+  PRINT_DEBUG("============================================\n");
 
 #ifdef HAVE_PYTHONLIBS
 
diff --git a/ov_eval/src/error_dataset.cpp b/ov_eval/src/error_dataset.cpp
index 3065dfc39..62188b143 100644
--- a/ov_eval/src/error_dataset.cpp
+++ b/ov_eval/src/error_dataset.cpp
@@ -26,6 +26,7 @@
 #include "calc/ResultTrajectory.h"
 #include "utils/Colors.h"
 #include "utils/Loader.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -40,9 +41,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 4) {
-    printf(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET);
-    printf(RED "ERROR: ./error_dataset <align_mode> <file_gt.txt> <folder_algorithms>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval error_dataset <align_mode> <file_gt.txt> <folder_algorithms>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_dataset <align_mode> <file_gt.txt> <folder_algorithms>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_dataset <align_mode> <file_gt.txt> <folder_algorithms>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -54,7 +55,7 @@ int main(int argc, char **argv) {
   ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos);
   // Print its length and stats
   double length = ov_eval::Loader::get_total_length(poses);
-  printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
+  PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
 
   // Get the algorithms we will process
   // Also create empty statistic objects for each of our datasets
@@ -83,8 +84,8 @@ int main(int argc, char **argv) {
   for (size_t i = 0; i < path_algorithms.size(); i++) {
 
     // Debug print
-    printf("======================================\n");
-    printf("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str());
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str());
 
     // Get the list of datasets this algorithm records
     std::map<std::string, boost::filesystem::path> path_algo_datasets;
@@ -96,8 +97,8 @@ int main(int argc, char **argv) {
 
     // Check if we have runs for our dataset
     if (path_algo_datasets.find(path_gt.stem().string()) == path_algo_datasets.end()) {
-      printf(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(),
-             path_gt.stem().c_str());
+      PRINT_DEBUG(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(),
+                  path_gt.stem().c_str());
       continue;
     }
 
@@ -123,7 +124,7 @@ int main(int argc, char **argv) {
 
     // Check if we have runs
     if (file_paths.empty()) {
-      printf(RED "\tERROR: No runs found for %s, is the folder structure right??\n" RESET, path_algorithms.at(i).filename().c_str());
+      PRINT_ERROR(RED "\tERROR: No runs found for %s, is the folder structure right??\n" RESET, path_algorithms.at(i).filename().c_str());
       continue;
     }
 
@@ -188,18 +189,18 @@ int main(int argc, char **argv) {
 
     // Print stats for this specific dataset
     std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : "";
-    printf("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean,
-           (int)ate_dataset_ori.values.size());
-    printf("\tATE: std_ori  = %.5f | std_pos  = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std);
-    printf("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean,
-           (int)ate_2d_dataset_ori.values.size());
-    printf("\tATE 2D: std_ori  = %.5f | std_pos  = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std);
+    PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean,
+                (int)ate_dataset_ori.values.size());
+    PRINT_DEBUG("\tATE: std_ori  = %.5f | std_pos  = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std);
+    PRINT_DEBUG("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean,
+                (int)ate_2d_dataset_ori.values.size());
+    PRINT_DEBUG("\tATE 2D: std_ori  = %.5f | std_pos  = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std);
     for (auto &seg : rpe_dataset) {
       seg.second.first.calculate();
       seg.second.second.calculate();
-      printf("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean,
-             seg.second.second.mean, (int)seg.second.second.values.size());
-      // printf("RPE: seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
+      PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean,
+                  seg.second.second.mean, (int)seg.second.second.values.size());
+      // PRINT_DEBUG("RPE: seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
     }
 
     // RMSE: Convert into the right format (only use times where all runs have an error)
@@ -216,7 +217,7 @@ int main(int argc, char **argv) {
     }
     rmse_ori.calculate();
     rmse_pos.calculate();
-    printf("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean);
+    PRINT_DEBUG("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean);
 
     // RMSE: Convert into the right format (only use times where all runs have an error)
     ov_eval::Statistics rmse_2d_ori, rmse_2d_pos;
@@ -232,7 +233,7 @@ int main(int argc, char **argv) {
     }
     rmse_2d_ori.calculate();
     rmse_2d_pos.calculate();
-    printf("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean);
+    PRINT_DEBUG("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean);
 
     // NEES: Convert into the right format (only use times where all runs have an error)
     ov_eval::Statistics nees_ori, nees_pos;
@@ -248,7 +249,7 @@ int main(int argc, char **argv) {
     }
     nees_ori.calculate();
     nees_pos.calculate();
-    printf("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
+    PRINT_DEBUG("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -315,7 +316,7 @@ int main(int argc, char **argv) {
   }
 
   // Final line for our printed stats
-  printf("============================================\n");
+  PRINT_DEBUG("============================================\n");
 
 #ifdef HAVE_PYTHONLIBS
 
diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp
index 757a6e247..2f732dcd2 100644
--- a/ov_eval/src/error_simulation.cpp
+++ b/ov_eval/src/error_simulation.cpp
@@ -21,6 +21,7 @@
 
 #include "calc/ResultSimulation.h"
 #include "utils/Colors.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -35,8 +36,8 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 4) {
-    printf(RED "ERROR: ./error_simulation <file_est.txt> <file_std.txt> <file_gt.txt>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval error_simulation <file_est.txt> <file_std.txt> <file_gt.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_simulation <file_est.txt> <file_std.txt> <file_gt.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation <file_est.txt> <file_std.txt> <file_gt.txt>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -44,19 +45,19 @@ int main(int argc, char **argv) {
   ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]);
 
   // Plot the state errors
-  printf("Plotting state variable errors...\n");
+  PRINT_INFO("Plotting state variable errors...\n");
   traj.plot_state(true);
 
   // Plot time offset
-  printf("Plotting time offset error...\n");
+  PRINT_INFO("Plotting time offset error...\n");
   traj.plot_timeoff(true, 10);
 
   // Plot camera intrinsics
-  printf("Plotting camera intrinsics...\n");
+  PRINT_INFO("Plotting camera intrinsics...\n");
   traj.plot_cam_instrinsics(true, 60);
 
   // Plot camera extrinsics
-  printf("Plotting camera extrinsics...\n");
+  PRINT_INFO("Plotting camera extrinsics...\n");
   traj.plot_cam_extrinsics(true, 60);
 
 #ifdef HAVE_PYTHONLIBS
diff --git a/ov_eval/src/error_singlerun.cpp b/ov_eval/src/error_singlerun.cpp
index 179bec23c..90699360d 100644
--- a/ov_eval/src/error_singlerun.cpp
+++ b/ov_eval/src/error_singlerun.cpp
@@ -25,6 +25,7 @@
 
 #include "calc/ResultTrajectory.h"
 #include "utils/Colors.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -85,9 +86,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 4) {
-    printf(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET);
-    printf(RED "ERROR: ./error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -99,7 +100,7 @@ int main(int argc, char **argv) {
   ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos);
   // Print its length and stats
   double length = ov_eval::Loader::get_total_length(poses);
-  printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
+  PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
 
   // Create our trajectory object
   ov_eval::ResultTrajectory traj(argv[3], argv[2], argv[1]);
@@ -113,14 +114,14 @@ int main(int argc, char **argv) {
   traj.calculate_ate(error_ori, error_pos);
 
   // Print it
-  printf("======================================\n");
-  printf("Absolute Trajectory Error\n");
-  printf("======================================\n");
-  printf("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse);
-  printf("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean);
-  printf("min_ori  = %.3f | min_pos  = %.3f\n", error_ori.min, error_pos.min);
-  printf("max_ori  = %.3f | max_pos  = %.3f\n", error_ori.max, error_pos.max);
-  printf("std_ori  = %.3f | std_pos  = %.3f\n", error_ori.std, error_pos.std);
+  PRINT_DEBUG("======================================\n");
+  PRINT_DEBUG("Absolute Trajectory Error\n");
+  PRINT_DEBUG("======================================\n");
+  PRINT_DEBUG("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse);
+  PRINT_DEBUG("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean);
+  PRINT_DEBUG("min_ori  = %.3f | min_pos  = %.3f\n", error_ori.min, error_pos.min);
+  PRINT_DEBUG("max_ori  = %.3f | max_pos  = %.3f\n", error_ori.max, error_pos.max);
+  PRINT_DEBUG("std_ori  = %.3f | std_pos  = %.3f\n", error_ori.std, error_pos.std);
 
   //===========================================================
   // Relative pose error
@@ -132,13 +133,13 @@ int main(int argc, char **argv) {
   traj.calculate_rpe(segments, error_rpe);
 
   // Print it
-  printf("======================================\n");
-  printf("Relative Pose Error\n");
-  printf("======================================\n");
+  PRINT_DEBUG("======================================\n");
+  PRINT_DEBUG("Relative Pose Error\n");
+  PRINT_DEBUG("======================================\n");
   for (const auto &seg : error_rpe) {
-    printf("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median,
-           seg.second.second.median, (int)seg.second.second.values.size());
-    // printf("seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
+    PRINT_DEBUG("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median,
+                seg.second.second.median, (int)seg.second.second.values.size());
+    // PRINT_DEBUG("seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
   }
 
 #ifdef HAVE_PYTHONLIBS
@@ -198,14 +199,14 @@ int main(int argc, char **argv) {
   traj.calculate_nees(nees_ori, nees_pos);
 
   // Print it
-  printf("======================================\n");
-  printf("Normalized Estimation Error Squared\n");
-  printf("======================================\n");
-  printf("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
-  printf("min_ori  = %.3f | min_pos  = %.3f\n", nees_ori.min, nees_pos.min);
-  printf("max_ori  = %.3f | max_pos  = %.3f\n", nees_ori.max, nees_pos.max);
-  printf("std_ori  = %.3f | std_pos  = %.3f\n", nees_ori.std, nees_pos.std);
-  printf("======================================\n");
+  PRINT_DEBUG("======================================\n");
+  PRINT_DEBUG("Normalized Estimation Error Squared\n");
+  PRINT_DEBUG("======================================\n");
+  PRINT_DEBUG("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
+  PRINT_DEBUG("min_ori  = %.3f | min_pos  = %.3f\n", nees_ori.min, nees_pos.min);
+  PRINT_DEBUG("max_ori  = %.3f | max_pos  = %.3f\n", nees_ori.max, nees_pos.max);
+  PRINT_DEBUG("std_ori  = %.3f | std_pos  = %.3f\n", nees_ori.std, nees_pos.std);
+  PRINT_DEBUG("======================================\n");
 
 #ifdef HAVE_PYTHONLIBS
 
diff --git a/ov_eval/src/format_converter.cpp b/ov_eval/src/format_converter.cpp
index 38f9451ee..f4bc52186 100644
--- a/ov_eval/src/format_converter.cpp
+++ b/ov_eval/src/format_converter.cpp
@@ -24,9 +24,11 @@
 #include <boost/filesystem.hpp>
 #include <fstream>
 #include <iostream>
+#include <sstream>
 #include <string>
 
 #include "utils/Colors.h"
+#include "utils/print.h"
 
 /**
  * Given a CSV file this will convert it to our text file format.
@@ -37,12 +39,12 @@ void process_csv(std::string infile) {
   std::ifstream file1;
   std::string line;
   file1.open(infile);
-  printf("Opening file %s\n", boost::filesystem::path(infile).filename().c_str());
+  PRINT_INFO("Opening file %s\n", boost::filesystem::path(infile).filename().c_str());
 
   // Check that it was successful
   if (!file1) {
-    printf(RED "ERROR: Unable to open input file...\n" RESET);
-    printf(RED "ERROR: %s\n" RESET, infile.c_str());
+    PRINT_ERROR(RED "ERROR: Unable to open input file...\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, infile.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -74,7 +76,9 @@ void process_csv(std::string infile) {
     // Only a valid line if we have all the parameters
     if (i > 7) {
       traj_data.push_back(data);
-      // std::cout << std::setprecision(5) << data.transpose() << std::endl;
+      // std::stringstream ss;
+      // ss << std::setprecision(5) << data.transpose() << std::endl;
+      // PRINT_DEBUG(ss.str().c_str());
     }
   }
 
@@ -83,17 +87,17 @@ void process_csv(std::string infile) {
 
   // Error if we don't have any data
   if (traj_data.empty()) {
-    printf(RED "ERROR: Could not parse any data from the file!!\n" RESET);
-    printf(RED "ERROR: %s\n" RESET, infile.c_str());
+    PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, infile.c_str());
     std::exit(EXIT_FAILURE);
   }
-  printf("\t- Loaded %d poses from file\n", (int)traj_data.size());
+  PRINT_INFO("\t- Loaded %d poses from file\n", (int)traj_data.size());
 
   // If file exists already then crash
   std::string outfile = infile.substr(0, infile.find_last_of('.')) + ".txt";
   if (boost::filesystem::exists(outfile)) {
-    printf(RED "\t- ERROR: Output file already exists, please delete and re-run this script!!\n" RESET);
-    printf(RED "\t- ERROR: %s\n" RESET, outfile.c_str());
+    PRINT_ERROR(RED "\t- ERROR: Output file already exists, please delete and re-run this script!!\n" RESET);
+    PRINT_ERROR(RED "\t- ERROR: %s\n" RESET, outfile.c_str());
     return;
   }
 
@@ -101,8 +105,8 @@ void process_csv(std::string infile) {
   std::ofstream file2;
   file2.open(outfile.c_str());
   if (file2.fail()) {
-    printf(RED "ERROR: Unable to open output file!!\n" RESET);
-    printf(RED "ERROR: %s\n" RESET, outfile.c_str());
+    PRINT_ERROR(RED "ERROR: Unable to open output file!!\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, outfile.c_str());
     std::exit(EXIT_FAILURE);
   }
   file2 << "# timestamp(s) tx ty tz qx qy qz qw" << std::endl;
@@ -116,7 +120,7 @@ void process_csv(std::string infile) {
     file2 << traj_data.at(i)(1) << " " << traj_data.at(i)(2) << " " << traj_data.at(i)(3) << " " << traj_data.at(i)(5) << " "
           << traj_data.at(i)(6) << " " << traj_data.at(i)(7) << " " << traj_data.at(i)(4) << std::endl;
   }
-  printf("\t- Saved to file %s\n", boost::filesystem::path(outfile).filename().c_str());
+  PRINT_INFO("\t- Saved to file %s\n", boost::filesystem::path(outfile).filename().c_str());
 
   // Finally close the file
   file2.close();
@@ -126,9 +130,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 2) {
-    printf(RED "ERROR: Please specify a file to convert\n" RESET);
-    printf(RED "ERROR: ./format_convert <file.csv or folder\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval format_convert <file.csv or folder>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a file to convert\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./format_convert <file.csv or folder\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval format_convert <file.csv or folder>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
diff --git a/ov_eval/src/live_align_trajectory.cpp b/ov_eval/src/live_align_trajectory.cpp
index 628793d67..44cf45917 100644
--- a/ov_eval/src/live_align_trajectory.cpp
+++ b/ov_eval/src/live_align_trajectory.cpp
@@ -28,6 +28,7 @@
 #include "utils/Colors.h"
 #include "utils/Loader.h"
 #include "utils/Math.h"
+#include "utils/print.h"
 
 ros::Publisher pub_path;
 void align_and_publish(const nav_msgs::Path::ConstPtr &msg);
@@ -93,8 +94,8 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) {
 
   // Return failure if we didn't have any common timestamps
   if (poses_temp.size() < 3) {
-    printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
-    printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
+    PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
+    PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
     return;
   }
 
@@ -104,8 +105,8 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) {
   double s_ESTtoGT;
   ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_type);
   Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT);
-  printf("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2),
-         q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
+  PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
+              q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
 
   // Finally lets calculate the aligned trajectories
   // TODO: maybe in the future we could live publish trajectory errors...
diff --git a/ov_eval/src/plot_trajectories.cpp b/ov_eval/src/plot_trajectories.cpp
index ea49c690f..7e8ac673b 100644
--- a/ov_eval/src/plot_trajectories.cpp
+++ b/ov_eval/src/plot_trajectories.cpp
@@ -32,6 +32,7 @@
 #include "utils/Colors.h"
 #include "utils/Loader.h"
 #include "utils/Math.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -87,9 +88,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 3) {
-    printf(RED "ERROR: Please specify a align mode and trajectory file\n" RESET);
-    printf(RED "ERROR: ./plot_trajectories <align_mode> <file_gt.txt> <file_est1.txt> ...  <file_est9.txt>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval plot_trajectories <align_mode> <file_gt.txt> <file_est1.txt> ...  <file_est9.txt>\n" RESET);
+    PRINT_DEBUG(RED "ERROR: Please specify a align mode and trajectory file\n" RESET);
+    PRINT_DEBUG(RED "ERROR: ./plot_trajectories <align_mode> <file_gt.txt> <file_est1.txt> ...  <file_est9.txt>\n" RESET);
+    PRINT_DEBUG(RED "ERROR: rosrun ov_eval plot_trajectories <align_mode> <file_gt.txt> <file_est1.txt> ...  <file_est9.txt>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -115,8 +116,8 @@ int main(int argc, char **argv) {
 
       // Return failure if we didn't have any common timestamps
       if (poses_temp.size() < 3) {
-        printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
-        printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
+        PRINT_DEBUG(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
+        PRINT_DEBUG(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
         std::exit(EXIT_FAILURE);
       }
 
@@ -128,8 +129,8 @@ int main(int argc, char **argv) {
 
       // Debug print to the user
       Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT);
-      printf("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
-             q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
+      PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
+                  q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
 
       // Finally lets calculate the aligned trajectories
       std::vector<Eigen::Matrix<double, 7, 1>> est_poses_aignedtoGT;
@@ -148,7 +149,7 @@ int main(int argc, char **argv) {
     boost::filesystem::path path(argv[i]);
     std::string name = path.stem().string();
     double length = ov_eval::Loader::get_total_length(poses_temp);
-    printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length);
+    PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length);
 
     // Save this to our arrays
     names.push_back(name);
diff --git a/ov_eval/src/timing_comparison.cpp b/ov_eval/src/timing_comparison.cpp
index 41bd29288..f5b161d84 100644
--- a/ov_eval/src/timing_comparison.cpp
+++ b/ov_eval/src/timing_comparison.cpp
@@ -30,6 +30,7 @@
 #include "utils/Colors.h"
 #include "utils/Loader.h"
 #include "utils/Statistics.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -44,9 +45,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 2) {
-    printf(RED "ERROR: Please specify a timing file\n" RESET);
-    printf(RED "ERROR: ./timing_comparison <file_times1.txt> ... <file_timesN.txt>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval timing_comparison <file_times1.txt> ... <file_timesN.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./timing_comparison <file_times1.txt> ... <file_timesN.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_comparison <file_times1.txt> ... <file_timesN.txt>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -58,15 +59,15 @@ int main(int argc, char **argv) {
     // Parse the name of this timing
     boost::filesystem::path path(argv[z]);
     std::string name = path.stem().string();
-    printf("======================================\n");
-    printf("[TIME]: loading data for %s\n", name.c_str());
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[TIME]: loading data for %s\n", name.c_str());
 
     // Load it!!
     std::vector<std::string> names_temp;
     std::vector<double> times;
     std::vector<Eigen::VectorXd> timing_values;
     ov_eval::Loader::load_timing_flamegraph(argv[z], names_temp, times, timing_values);
-    printf("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names_temp.size());
+    PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names_temp.size());
 
     // Our categories
     std::vector<ov_eval::Statistics> stats;
@@ -84,8 +85,8 @@ int main(int argc, char **argv) {
     // Now print the statistic for this run
     for (size_t i = 0; i < names_temp.size(); i++) {
       stats.at(i).calculate();
-      printf("mean_time = %.4f | std = %.4f | 99th = %.4f  | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, stats.at(i).ninetynine,
-             stats.at(i).max, names_temp.at(i).c_str());
+      PRINT_DEBUG("mean_time = %.4f | std = %.4f | 99th = %.4f  | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std,
+                  stats.at(i).ninetynine, stats.at(i).max, names_temp.at(i).c_str());
     }
 
     // Append the total stats to the big vector
@@ -93,9 +94,9 @@ int main(int argc, char **argv) {
       names.push_back(name);
       total_times.push_back(stats.at(stats.size() - 1));
     } else {
-      printf(RED "[TIME]: unable to load any data.....\n" RESET);
+      PRINT_DEBUG(RED "[TIME]: unable to load any data.....\n" RESET);
     }
-    printf("======================================\n");
+    PRINT_DEBUG("======================================\n");
   }
 
 #ifdef HAVE_PYTHONLIBS
diff --git a/ov_eval/src/timing_flamegraph.cpp b/ov_eval/src/timing_flamegraph.cpp
index 4dea53d6a..f374cb7af 100644
--- a/ov_eval/src/timing_flamegraph.cpp
+++ b/ov_eval/src/timing_flamegraph.cpp
@@ -30,6 +30,7 @@
 #include "utils/Colors.h"
 #include "utils/Loader.h"
 #include "utils/Statistics.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -44,9 +45,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 2) {
-    printf(RED "ERROR: Please specify a timing file\n" RESET);
-    printf(RED "ERROR: ./timing_flamegraph <file_times.txt>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval timing_flamegraph <file_times.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./timing_flamegraph <file_times.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_flamegraph <file_times.txt>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -55,7 +56,7 @@ int main(int argc, char **argv) {
   std::vector<double> times;
   std::vector<Eigen::VectorXd> timing_values;
   ov_eval::Loader::load_timing_flamegraph(argv[1], names, times, timing_values);
-  printf("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size());
+  PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size());
 
   // Our categories
   std::vector<ov_eval::Statistics> stats;
@@ -73,8 +74,8 @@ int main(int argc, char **argv) {
   // Now print the statistic for this run
   for (size_t i = 0; i < names.size(); i++) {
     stats.at(i).calculate();
-    printf("mean_time = %.4f | std = %.4f | 99th = %.4f  | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, stats.at(i).ninetynine,
-           stats.at(i).max, names.at(i).c_str());
+    PRINT_DEBUG("mean_time = %.4f | std = %.4f | 99th = %.4f  | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std,
+                stats.at(i).ninetynine, stats.at(i).max, names.at(i).c_str());
   }
 
 #ifdef HAVE_PYTHONLIBS
diff --git a/ov_eval/src/timing_percentages.cpp b/ov_eval/src/timing_percentages.cpp
index 79dc8d8bb..e163350fc 100644
--- a/ov_eval/src/timing_percentages.cpp
+++ b/ov_eval/src/timing_percentages.cpp
@@ -30,6 +30,7 @@
 #include "utils/Colors.h"
 #include "utils/Loader.h"
 #include "utils/Statistics.h"
+#include "utils/print.h"
 
 #ifdef HAVE_PYTHONLIBS
 
@@ -44,9 +45,9 @@ int main(int argc, char **argv) {
 
   // Ensure we have a path
   if (argc < 2) {
-    printf(RED "ERROR: Please specify a timing and memory percent folder\n" RESET);
-    printf(RED "ERROR: ./timing_percentages <timings_folder>\n" RESET);
-    printf(RED "ERROR: rosrun ov_eval timing_percentages <timings_folder>\n" RESET);
+    PRINT_ERROR(RED "ERROR: Please specify a timing and memory percent folder\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./timing_percentages <timings_folder>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_percentages <timings_folder>\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -76,8 +77,8 @@ int main(int argc, char **argv) {
   for (size_t i = 0; i < path_algorithms.size(); i++) {
 
     // Debug print
-    printf("======================================\n");
-    printf("[COMP]: processing %s algorithm\n", path_algorithms.at(i).stem().c_str());
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).stem().c_str());
 
     // our total summed values
     std::vector<double> total_times;
@@ -117,14 +118,14 @@ int main(int argc, char **argv) {
     }
 
     // Display for the user
-    printf("\tloaded %d timestamps from file!!\n", (int)algo_timings.at(algo).at(0).timestamps.size());
+    PRINT_DEBUG("\tloaded %d timestamps from file!!\n", (int)algo_timings.at(algo).at(0).timestamps.size());
     algo_timings.at(algo).at(0).calculate();
     algo_timings.at(algo).at(1).calculate();
     algo_timings.at(algo).at(2).calculate();
-    printf("\tPREC: mean_cpu = %.3f +- %.3f\n", algo_timings.at(algo).at(0).mean, algo_timings.at(algo).at(0).std);
-    printf("\tPREC: mean_mem = %.3f +- %.3f\n", algo_timings.at(algo).at(1).mean, algo_timings.at(algo).at(1).std);
-    printf("\tTHR: mean_threads = %.3f +- %.3f\n", algo_timings.at(algo).at(2).mean, algo_timings.at(algo).at(2).std);
-    printf("======================================\n");
+    PRINT_DEBUG("\tPREC: mean_cpu = %.3f +- %.3f\n", algo_timings.at(algo).at(0).mean, algo_timings.at(algo).at(0).std);
+    PRINT_DEBUG("\tPREC: mean_mem = %.3f +- %.3f\n", algo_timings.at(algo).at(1).mean, algo_timings.at(algo).at(1).std);
+    PRINT_DEBUG("\tTHR: mean_threads = %.3f +- %.3f\n", algo_timings.at(algo).at(2).mean, algo_timings.at(algo).at(2).std);
+    PRINT_DEBUG("======================================\n");
   }
 
   //===============================================================================
diff --git a/ov_eval/src/utils/Loader.cpp b/ov_eval/src/utils/Loader.cpp
index ab45d21aa..9f7e0edf3 100644
--- a/ov_eval/src/utils/Loader.cpp
+++ b/ov_eval/src/utils/Loader.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "Loader.h"
+#include "utils/print.h"
 
 using namespace ov_eval;
 
@@ -29,8 +30,8 @@ void Loader::load_data(std::string path_traj, std::vector<double> &times, std::v
   // Try to open our trajectory file
   std::ifstream file(path_traj);
   if (!file.is_open()) {
-    printf(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -82,28 +83,28 @@ void Loader::load_data(std::string path_traj, std::vector<double> &times, std::v
 
   // Error if we don't have any data
   if (times.empty()) {
-    printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
   // Assert that they are all equal
   if (times.size() != poses.size()) {
-    printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
   // Assert that they are all equal
   if (!cov_ori.empty() && (times.size() != cov_ori.size() || times.size() != cov_pos.size())) {
-    printf(RED "[LOAD]: Parsing error, timestamps covariance size do not match!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Parsing error, timestamps covariance size do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
   // Debug print amount
   // std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1);
-  // printf("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str());
+  // PRINT_DEBUG("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str());
 }
 
 void Loader::load_data_csv(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
@@ -112,8 +113,8 @@ void Loader::load_data_csv(std::string path_traj, std::vector<double> &times, st
   // Try to open our trajectory file
   std::ifstream file(path_traj);
   if (!file.is_open()) {
-    printf(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -163,15 +164,15 @@ void Loader::load_data_csv(std::string path_traj, std::vector<double> &times, st
 
   // Error if we don't have any data
   if (times.empty()) {
-    printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
   // Assert that they are all equal
   if (times.size() != poses.size()) {
-    printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 }
@@ -181,8 +182,8 @@ void Loader::load_simulation(std::string path, std::vector<Eigen::VectorXd> &val
   // Try to open our trajectory file
   std::ifstream file(path);
   if (!file.is_open()) {
-    printf(RED "[LOAD]: Unable to open file...\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Unable to open file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -221,8 +222,8 @@ void Loader::load_simulation(std::string path, std::vector<Eigen::VectorXd> &val
 
   // Error if we don't have any data
   if (values.empty()) {
-    printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -230,8 +231,8 @@ void Loader::load_simulation(std::string path, std::vector<Eigen::VectorXd> &val
   int rowsize = values.at(0).rows();
   for (size_t i = 0; i < values.size(); i++) {
     if (values.at(i).rows() != rowsize) {
-      printf(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)values.at(i).rows(), rowsize);
-      printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+      PRINT_ERROR(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)values.at(i).rows(), rowsize);
+      PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
       std::exit(EXIT_FAILURE);
     }
   }
@@ -243,8 +244,8 @@ void Loader::load_timing_flamegraph(std::string path, std::vector<std::string> &
   // Try to open our trajectory file
   std::ifstream file(path);
   if (!file.is_open()) {
-    printf(RED "[LOAD]: Unable to open file...\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Unable to open file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -301,8 +302,8 @@ void Loader::load_timing_flamegraph(std::string path, std::vector<std::string> &
 
   // Error if we don't have any data
   if (timing_values.empty()) {
-    printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -310,9 +311,9 @@ void Loader::load_timing_flamegraph(std::string path, std::vector<std::string> &
   int rowsize = names.size();
   for (size_t i = 0; i < timing_values.size(); i++) {
     if (timing_values.at(i).rows() != rowsize) {
-      printf(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)timing_values.at(i).rows(),
-             rowsize);
-      printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+      PRINT_ERROR(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)timing_values.at(i).rows(),
+                  rowsize);
+      PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
       std::exit(EXIT_FAILURE);
     }
   }
@@ -324,8 +325,8 @@ void Loader::load_timing_percent(std::string path, std::vector<double> &times, s
   // Try to open our trajectory file
   std::ifstream file(path);
   if (!file.is_open()) {
-    printf(RED "[LOAD]: Unable to open timing file...\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Unable to open timing file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 
@@ -372,15 +373,15 @@ void Loader::load_timing_percent(std::string path, std::vector<double> &times, s
 
   // Error if we don't have any data
   if (times.empty()) {
-    printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 
   // Assert that they are all equal
   if (times.size() != summed_values.size() || times.size() != node_values.size()) {
-    printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
-    printf(RED "[LOAD]: %s\n" RESET, path.c_str());
+    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
     std::exit(EXIT_FAILURE);
   }
 }
diff --git a/ov_eval/src/utils/Math.h b/ov_eval/src/utils/Math.h
index 6cc8fc81f..e37439a2d 100644
--- a/ov_eval/src/utils/Math.h
+++ b/ov_eval/src/utils/Math.h
@@ -27,6 +27,8 @@
 #include <sstream>
 #include <string>
 
+#include "utils/print.h"
+
 using namespace std;
 
 namespace ov_eval {
@@ -65,26 +67,26 @@ class Math {
     Eigen::Matrix<double, 4, 1> q;
     double T = rot.trace();
     if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) {
-      // cout << "case 1- " << endl;
+      // PRINT_DEBUG(("case 1- \n");
       q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4);
       q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0));
       q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0));
       q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1));
 
     } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) {
-      // cout << "case 2- " << endl;
+      // PRINT_DEBUG(("case 2- \n");
       q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4);
       q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0));
       q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1));
       q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2));
     } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) {
-      // cout << "case 3- " << endl;
+      // PRINT_DEBUG(("case 3- \n");
       q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4);
       q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0));
       q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1));
       q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0));
     } else {
-      // cout << "case 4- " << endl;
+      // PRINT_DEBUG(("case 4- \n");
       q(3) = sqrt((1 + T) / 4);
       q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1));
       q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2));
diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp
index e13524aad..000988c6f 100644
--- a/ov_msckf/src/core/RosVisualizer.cpp
+++ b/ov_msckf/src/core/RosVisualizer.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "RosVisualizer.h"
+#include "utils/print.h"
 
 using namespace ov_msckf;
 
@@ -72,7 +73,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr<VioManager> ap
   if (nh.hasParam("path_gt") && _sim == nullptr) {
     std::string path_to_gt;
     nh.param<std::string>("path_gt", path_to_gt, "");
-    if(!path_to_gt.empty()) {
+    if (!path_to_gt.empty()) {
       DatasetReader::load_gt_file(path_to_gt, gt_states);
       ROS_INFO("gt file path is: %s", path_to_gt.c_str());
     }
@@ -155,7 +156,7 @@ void RosVisualizer::visualize() {
   // Print how much time it took to publish / displaying things
   rT0_2 = boost::posix_time::microsec_clock::local_time();
   double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6;
-  printf(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total);
+  PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total);
 }
 
 void RosVisualizer::visualize_odometry(double timestamp) {
@@ -231,16 +232,16 @@ void RosVisualizer::visualize_final() {
 
   // Final time offset value
   if (_app->get_state()->_options.do_calib_camera_timeoffset) {
-    printf(REDPURPLE "camera-imu timeoffset = %.5f\n\n" RESET, _app->get_state()->_calib_dt_CAMtoIMU->value()(0));
+    PRINT_INFO(REDPURPLE "camera-imu timeoffset = %.5f\n\n" RESET, _app->get_state()->_calib_dt_CAMtoIMU->value()(0));
   }
 
   // Final camera intrinsics
   if (_app->get_state()->_options.do_calib_camera_intrinsics) {
     for (int i = 0; i < _app->get_state()->_options.num_cameras; i++) {
       std::shared_ptr<Vec> calib = _app->get_state()->_cam_intrinsics.at(i);
-      printf(REDPURPLE "cam%d intrinsics:\n" RESET, (int)i);
-      printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n" RESET, calib->value()(0), calib->value()(1), calib->value()(2), calib->value()(3));
-      printf(REDPURPLE "%.5f,%.5f,%.5f,%.5f\n\n" RESET, calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7));
+      PRINT_INFO(REDPURPLE "cam%d intrinsics:\n" RESET, (int)i);
+      PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n" RESET, calib->value()(0), calib->value()(1), calib->value()(2), calib->value()(3));
+      PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,%.5f\n\n" RESET, calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7));
     }
   }
 
@@ -251,31 +252,31 @@ void RosVisualizer::visualize_final() {
       Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity();
       T_CtoI.block(0, 0, 3, 3) = quat_2_Rot(calib->quat()).transpose();
       T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * calib->pos();
-      printf(REDPURPLE "T_C%dtoI:\n" RESET, i);
-      printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(0, 0), T_CtoI(0, 1), T_CtoI(0, 2), T_CtoI(0, 3));
-      printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(1, 0), T_CtoI(1, 1), T_CtoI(1, 2), T_CtoI(1, 3));
-      printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(2, 0), T_CtoI(2, 1), T_CtoI(2, 2), T_CtoI(2, 3));
-      printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n\n" RESET, T_CtoI(3, 0), T_CtoI(3, 1), T_CtoI(3, 2), T_CtoI(3, 3));
+      PRINT_INFO(REDPURPLE "T_C%dtoI:\n" RESET, i);
+      PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(0, 0), T_CtoI(0, 1), T_CtoI(0, 2), T_CtoI(0, 3));
+      PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(1, 0), T_CtoI(1, 1), T_CtoI(1, 2), T_CtoI(1, 3));
+      PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(2, 0), T_CtoI(2, 1), T_CtoI(2, 2), T_CtoI(2, 3));
+      PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n\n" RESET, T_CtoI(3, 0), T_CtoI(3, 1), T_CtoI(3, 2), T_CtoI(3, 3));
     }
   }
 
   // Publish RMSE if we have it
   if (!gt_states.empty()) {
-    printf(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number);
-    printf(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number);
+    PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number);
+    PRINT_INFO(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number);
   }
 
   // Publish RMSE and NEES if doing simulation
   if (_sim != nullptr) {
-    printf(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number);
-    printf(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number);
-    printf(REDPURPLE "NEES average: %.3f (deg) orientation\n" RESET, summed_nees_ori / summed_number);
-    printf(REDPURPLE "NEES average: %.3f (m) position\n\n" RESET, summed_nees_pos / summed_number);
+    PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number);
+    PRINT_INFO(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number);
+    PRINT_INFO(REDPURPLE "NEES average: %.3f (deg) orientation\n" RESET, summed_nees_ori / summed_number);
+    PRINT_INFO(REDPURPLE "NEES average: %.3f (m) position\n\n" RESET, summed_nees_pos / summed_number);
   }
 
   // Print the total time
   rT2 = boost::posix_time::microsec_clock::local_time();
-  printf(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6);
+  PRINT_INFO(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6);
 }
 
 void RosVisualizer::publish_state() {
@@ -667,10 +668,10 @@ void RosVisualizer::publish_groundtruth() {
   }
 
   // Nice display for the user
-  printf(REDPURPLE "error to gt => %.3f, %.3f (deg,m) | average error => %.3f, %.3f (deg,m) | called %d times\n" RESET, rmse_ori, rmse_pos,
-         summed_rmse_ori / summed_number, summed_rmse_pos / summed_number, (int)summed_number);
-  printf(REDPURPLE "nees => %.1f, %.1f (ori,pos) | average nees = %.1f, %.1f (ori,pos)\n" RESET, ori_nees, pos_nees,
-         summed_nees_ori / summed_number, summed_nees_pos / summed_number);
+  PRINT_INFO(REDPURPLE "error to gt => %.3f, %.3f (deg,m) | average error => %.3f, %.3f (deg,m) | called %d times\n" RESET, rmse_ori,
+             rmse_pos, summed_rmse_ori / summed_number, summed_rmse_pos / summed_number, (int)summed_number);
+  PRINT_INFO(REDPURPLE "nees => %.1f, %.1f (ori,pos) | average nees = %.1f, %.1f (ori,pos)\n" RESET, ori_nees, pos_nees,
+             summed_nees_ori / summed_number, summed_nees_pos / summed_number);
 
   //==========================================================================
   //==========================================================================
diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp
index f3c2a202c..abfe7e68c 100644
--- a/ov_msckf/src/core/VioManager.cpp
+++ b/ov_msckf/src/core/VioManager.cpp
@@ -20,9 +20,11 @@
  */
 
 #include "VioManager.h"
-
 #include "types/Landmark.h"
+#include "utils/print.h"
+
 #include <memory>
+#include <sstream>
 
 using namespace ov_core;
 using namespace ov_type;
@@ -31,9 +33,9 @@ using namespace ov_msckf;
 VioManager::VioManager(VioManagerOptions &params_) {
 
   // Nice startup message
-  printf("=======================================\n");
-  printf("OPENVINS ON-MANIFOLD EKF IS STARTING\n");
-  printf("=======================================\n");
+  PRINT_DEBUG("=======================================\n");
+  PRINT_DEBUG("OPENVINS ON-MANIFOLD EKF IS STARTING\n");
+  PRINT_DEBUG("=======================================\n");
 
   // Nice debug
   this->params = params_;
@@ -87,7 +89,7 @@ VioManager::VioManager(VioManagerOptions &params_) {
     // If the file exists, then delete it
     if (boost::filesystem::exists(params.record_timing_filepath)) {
       boost::filesystem::remove(params.record_timing_filepath);
-      printf(YELLOW "[STATS]: found old file found, deleted...\n" RESET);
+      PRINT_INFO(YELLOW "[STATS]: found old file found, deleted...\n" RESET);
     }
     // Create the directory that we will open the file in
     boost::filesystem::path p(params.record_timing_filepath);
@@ -196,7 +198,7 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
     // Replace with the simulated tracker
     trackSIM = std::make_shared<TrackSIM>(state->_cam_intrinsics_cameras, state->_options.max_aruco_features);
     trackFEATS = trackSIM;
-    printf(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET);
+    PRINT_WARNING(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET);
   }
   trackSIM->set_width_height(params.camera_wh);
 
@@ -240,8 +242,8 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
 
   // If we do not have VIO initialization, then return an error
   if (!is_initialized_vio) {
-    printf(RED "[SIM]: your vio system should already be initialized before simulating features!!!\n" RESET);
-    printf(RED "[SIM]: initialize your system first before calling feed_measurement_simulation()!!!!\n" RESET);
+    PRINT_ERROR(RED "[SIM]: your vio system should already be initialized before simulating features!!!\n" RESET);
+    PRINT_ERROR(RED "[SIM]: initialize your system first before calling feed_measurement_simulation()!!!!\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -355,7 +357,8 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
 
   // Return if the camera measurement is out of order
   if (state->_timestamp > message.timestamp) {
-    printf(YELLOW "image received out of order, unable to do anything (prop dt = %3f)\n" RESET, (message.timestamp - state->_timestamp));
+    PRINT_WARNING(YELLOW "image received out of order, unable to do anything (prop dt = %3f)\n" RESET,
+                  (message.timestamp - state->_timestamp));
     return;
   }
 
@@ -372,14 +375,15 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   // This isn't super ideal, but it keeps the logic after this easier...
   // We can start processing things when we have at least 5 clones since we can start triangulating things...
   if ((int)state->_clones_IMU.size() < std::min(state->_options.max_clone_size, 5)) {
-    printf("waiting for enough clone states (%d of %d)....\n", (int)state->_clones_IMU.size(), std::min(state->_options.max_clone_size, 5));
+    PRINT_INFO("waiting for enough clone states (%d of %d)....\n", (int)state->_clones_IMU.size(),
+               std::min(state->_options.max_clone_size, 5));
     return;
   }
 
   // Return if we where unable to propagate
   if (state->_timestamp != message.timestamp) {
-    printf(RED "[PROP]: Propagator unable to propagate the state forward in time!\n" RESET);
-    printf(RED "[PROP]: It has been %.3f since last time we propagated\n" RESET, message.timestamp - state->_timestamp);
+    PRINT_WARNING(RED "[PROP]: Propagator unable to propagate the state forward in time!\n" RESET);
+    PRINT_WARNING(RED "[PROP]: It has been %.3f since last time we propagated\n" RESET, message.timestamp - state->_timestamp);
     return;
   }
   has_moved_since_zupt = true;
@@ -425,7 +429,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   it1 = feats_lost.begin();
   while (it1 != feats_lost.end()) {
     if (std::find(feats_marg.begin(), feats_marg.end(), (*it1)) != feats_marg.end()) {
-      // printf(YELLOW "FOUND FEATURE THAT WAS IN BOTH feats_lost and feats_marg!!!!!!\n" RESET);
+      // PRINT_WARNING(YELLOW "FOUND FEATURE THAT WAS IN BOTH feats_lost and feats_marg!!!!!!\n" RESET);
       it1 = feats_lost.erase(it1);
     } else {
       it1++;
@@ -507,11 +511,11 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   for (size_t i = 0; i < feats_slam.size(); i++) {
     if (state->_features_SLAM.find(feats_slam.at(i)->featid) != state->_features_SLAM.end()) {
       feats_slam_UPDATE.push_back(feats_slam.at(i));
-      // printf("[UPDATE-SLAM]: found old feature %d (%d
+      // PRINT_DEBUG("[UPDATE-SLAM]: found old feature %d (%d
       // measurements)\n",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size());
     } else {
       feats_slam_DELAYED.push_back(feats_slam.at(i));
-      // printf("[UPDATE-SLAM]: new feature ready %d (%d
+      // PRINT_DEBUG("[UPDATE-SLAM]: new feature ready %d (%d
       // measurements)\n",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size());
     }
   }
@@ -630,19 +634,22 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   double time_total = (rT7 - rT1).total_microseconds() * 1e-6;
 
   // Timing information
-  printf(BLUE "[TIME]: %.4f seconds for tracking\n" RESET, time_track);
-  printf(BLUE "[TIME]: %.4f seconds for propagation\n" RESET, time_prop);
-  printf(BLUE "[TIME]: %.4f seconds for MSCKF update (%d feats)\n" RESET, time_msckf, (int)featsup_MSCKF.size());
+  PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for tracking\n" RESET, time_track);
+  PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for propagation\n" RESET, time_prop);
+  PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for MSCKF update (%d feats)\n" RESET, time_msckf, (int)featsup_MSCKF.size());
   if (state->_options.max_slam_features > 0) {
-    printf(BLUE "[TIME]: %.4f seconds for SLAM update (%d feats)\n" RESET, time_slam_update, (int)state->_features_SLAM.size());
-    printf(BLUE "[TIME]: %.4f seconds for SLAM delayed init (%d feats)\n" RESET, time_slam_delay, (int)feats_slam_DELAYED.size());
+    PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for SLAM update (%d feats)\n" RESET, time_slam_update, (int)state->_features_SLAM.size());
+    PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for SLAM delayed init (%d feats)\n" RESET, time_slam_delay, (int)feats_slam_DELAYED.size());
   }
-  printf(BLUE "[TIME]: %.4f seconds for re-tri & marg (%d clones in state)\n" RESET, time_marg, (int)state->_clones_IMU.size());
-  printf(BLUE "[TIME]: %.4f seconds for total (camera" RESET, time_total);
+  PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for re-tri & marg (%d clones in state)\n" RESET, time_marg, (int)state->_clones_IMU.size());
+
+  std::stringstream ss;
+  ss << "[TIME]: " << std::setprecision(4) << time_total << " seconds for total (camera";
   for (const auto &id : message.sensor_ids) {
-    printf(BLUE " %d", id);
+    ss << " " << id;
   }
-  printf(")\n" RESET);
+  ss << ")" << std::endl;
+  PRINT_DEBUG(BLUE "%s" RESET, ss.str().c_str());
 
   // Finally if we are saving stats to file, lets save it to file
   if (params.record_timing_information && of_statistics.is_open()) {
@@ -668,22 +675,23 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   timelastupdate = message.timestamp;
 
   // Debug, print our current state
-  printf("q_GtoI = %.3f,%.3f,%.3f,%.3f | p_IinG = %.3f,%.3f,%.3f | dist = %.2f (meters)\n", state->_imu->quat()(0), state->_imu->quat()(1),
-         state->_imu->quat()(2), state->_imu->quat()(3), state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2), distance);
-  printf("bg = %.4f,%.4f,%.4f | ba = %.4f,%.4f,%.4f\n", state->_imu->bias_g()(0), state->_imu->bias_g()(1), state->_imu->bias_g()(2),
-         state->_imu->bias_a()(0), state->_imu->bias_a()(1), state->_imu->bias_a()(2));
+  PRINT_INFO("q_GtoI = %.3f,%.3f,%.3f,%.3f | p_IinG = %.3f,%.3f,%.3f | dist = %.2f (meters)\n", state->_imu->quat()(0),
+             state->_imu->quat()(1), state->_imu->quat()(2), state->_imu->quat()(3), state->_imu->pos()(0), state->_imu->pos()(1),
+             state->_imu->pos()(2), distance);
+  PRINT_INFO("bg = %.4f,%.4f,%.4f | ba = %.4f,%.4f,%.4f\n", state->_imu->bias_g()(0), state->_imu->bias_g()(1), state->_imu->bias_g()(2),
+             state->_imu->bias_a()(0), state->_imu->bias_a()(1), state->_imu->bias_a()(2));
 
   // Debug for camera imu offset
   if (state->_options.do_calib_camera_timeoffset) {
-    printf("camera-imu timeoffset = %.5f\n", state->_calib_dt_CAMtoIMU->value()(0));
+    PRINT_INFO("camera-imu timeoffset = %.5f\n", state->_calib_dt_CAMtoIMU->value()(0));
   }
 
   // Debug for camera intrinsics
   if (state->_options.do_calib_camera_intrinsics) {
     for (int i = 0; i < state->_options.num_cameras; i++) {
       std::shared_ptr<Vec> calib = state->_cam_intrinsics.at(i);
-      printf("cam%d intrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f,%.3f\n", (int)i, calib->value()(0), calib->value()(1),
-             calib->value()(2), calib->value()(3), calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7));
+      PRINT_INFO("cam%d intrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f,%.3f\n", (int)i, calib->value()(0), calib->value()(1),
+                 calib->value()(2), calib->value()(3), calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7));
     }
   }
 
@@ -691,8 +699,8 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   if (state->_options.do_calib_camera_pose) {
     for (int i = 0; i < state->_options.num_cameras; i++) {
       std::shared_ptr<PoseJPL> calib = state->_calib_IMUtoCAM.at(i);
-      printf("cam%d extrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f\n", (int)i, calib->quat()(0), calib->quat()(1), calib->quat()(2),
-             calib->quat()(3), calib->pos()(0), calib->pos()(1), calib->pos()(2));
+      PRINT_INFO("cam%d extrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f\n", (int)i, calib->quat()(0), calib->quat()(1), calib->quat()(2),
+                 calib->quat()(3), calib->pos()(0), calib->pos()(1), calib->pos()(2));
     }
   }
 }
@@ -740,14 +748,14 @@ bool VioManager::try_to_initialize() {
   }
 
   // Else we are good to go, print out our stats
-  printf(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1),
-         state->_imu->quat()(2), state->_imu->quat()(3));
-  printf(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1),
-         state->_imu->bias_g()(2));
-  printf(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2));
-  printf(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1),
-         state->_imu->bias_a()(2));
-  printf(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2));
+  PRINT_INFO(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1),
+             state->_imu->quat()(2), state->_imu->quat()(3));
+  PRINT_INFO(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1),
+             state->_imu->bias_g()(2));
+  PRINT_INFO(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2));
+  PRINT_INFO(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1),
+             state->_imu->bias_a()(2));
+  PRINT_INFO(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2));
   return true;
 }
 
@@ -905,7 +913,7 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message)
     // Skip if not valid (i.e. negative depth, or outside of image)
     if (uv_dist(0) < 0 || (int)uv_dist(0) >= params.camera_wh.at(0).first || uv_dist(1) < 0 ||
         (int)uv_dist(1) >= params.camera_wh.at(0).second) {
-      // printf("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1));
+      // PRINT_DEBUG("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1));
       continue;
     }
 
@@ -917,9 +925,9 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message)
   retri_rT5 = boost::posix_time::microsec_clock::local_time();
 
   // Timing information
-  // printf(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6);
-  // printf(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6);
-  // printf(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6);
-  // printf(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6);
-  // printf(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6);
+  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6);
+  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6);
+  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6);
 }
diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h
index 43702c9d9..d8e33bb2b 100644
--- a/ov_msckf/src/core/VioManager.h
+++ b/ov_msckf/src/core/VioManager.h
@@ -40,6 +40,7 @@
 #include "types/Landmark.h"
 #include "types/LandmarkRepresentation.h"
 #include "utils/lambda_body.h"
+#include "utils/print.h"
 #include "utils/sensor_data.h"
 
 #include "state/Propagator.h"
@@ -116,15 +117,15 @@ class VioManager {
     }
 
     // Print what we init'ed with
-    printf(GREEN "[INIT]: INITIALIZED FROM GROUNDTRUTH FILE!!!!!\n" RESET);
-    printf(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1),
-           state->_imu->quat()(2), state->_imu->quat()(3));
-    printf(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1),
-           state->_imu->bias_g()(2));
-    printf(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2));
-    printf(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1),
-           state->_imu->bias_a()(2));
-    printf(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2));
+    PRINT_DEBUG(GREEN "[INIT]: INITIALIZED FROM GROUNDTRUTH FILE!!!!!\n" RESET);
+    PRINT_DEBUG(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1),
+                state->_imu->quat()(2), state->_imu->quat()(3));
+    PRINT_DEBUG(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1),
+                state->_imu->bias_g()(2));
+    PRINT_DEBUG(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2));
+    PRINT_DEBUG(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1),
+                state->_imu->bias_a()(2));
+    PRINT_DEBUG(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2));
   }
 
   /// If we are initialized or not
diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h
index 70d336f0b..e009d5ff3 100644
--- a/ov_msckf/src/core/VioManagerOptions.h
+++ b/ov_msckf/src/core/VioManagerOptions.h
@@ -24,6 +24,7 @@
 
 #include <Eigen/Eigen>
 #include <iostream>
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -33,6 +34,7 @@
 #include "track/TrackBase.h"
 #include "update/UpdaterOptions.h"
 #include "utils/colors.h"
+#include "utils/print.h"
 #include "utils/quat_ops.h"
 
 using namespace std;
@@ -89,18 +91,18 @@ struct VioManagerOptions {
    * This allows for visual checking that everything was loaded properly from ROS/CMD parsers.
    */
   void print_estimator() {
-    printf("ESTIMATOR PARAMETERS:\n");
+    PRINT_INFO("ESTIMATOR PARAMETERS:\n");
     state_options.print();
-    printf("\t- dt_slam_delay: %.1f\n", dt_slam_delay);
-    printf("\t- init_window_time: %.2f\n", init_window_time);
-    printf("\t- init_imu_thresh: %.2f\n", init_imu_thresh);
-    printf("\t- zero_velocity_update: %d\n", try_zupt);
-    printf("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity);
-    printf("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier);
-    printf("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity);
-    printf("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning);
-    printf("\t- record timing?: %d\n", (int)record_timing_information);
-    printf("\t- record timing filepath: %s\n", record_timing_filepath.c_str());
+    PRINT_INFO("\t- dt_slam_delay: %.1f\n", dt_slam_delay);
+    PRINT_INFO("\t- init_window_time: %.2f\n", init_window_time);
+    PRINT_INFO("\t- init_imu_thresh: %.2f\n", init_imu_thresh);
+    PRINT_INFO("\t- zero_velocity_update: %d\n", try_zupt);
+    PRINT_INFO("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity);
+    PRINT_INFO("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier);
+    PRINT_INFO("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity);
+    PRINT_INFO("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning);
+    PRINT_INFO("\t- record timing?: %d\n", (int)record_timing_information);
+    PRINT_INFO("\t- record timing filepath: %s\n", record_timing_filepath.c_str());
   }
 
   // NOISE / CHI2 ============================
@@ -125,15 +127,15 @@ struct VioManagerOptions {
    * This allows for visual checking that everything was loaded properly from ROS/CMD parsers.
    */
   void print_noise() {
-    printf("NOISE PARAMETERS:\n");
+    PRINT_INFO("NOISE PARAMETERS:\n");
     imu_noises.print();
-    printf("\tUpdater MSCKF Feats:\n");
+    PRINT_INFO("\tUpdater MSCKF Feats:\n");
     msckf_options.print();
-    printf("\tUpdater SLAM Feats:\n");
+    PRINT_INFO("\tUpdater SLAM Feats:\n");
     slam_options.print();
-    printf("\tUpdater ARUCO Tags:\n");
+    PRINT_INFO("\tUpdater ARUCO Tags:\n");
     aruco_options.print();
-    printf("\tUpdater ZUPT:\n");
+    PRINT_INFO("\tUpdater ZUPT:\n");
     zupt_options.print();
   }
 
@@ -162,22 +164,24 @@ struct VioManagerOptions {
    * This allows for visual checking that everything was loaded properly from ROS/CMD parsers.
    */
   void print_state() {
-    printf("STATE PARAMETERS:\n");
-    printf("\t- gravity_mag: %.4f\n", gravity_mag);
-    printf("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag);
-    printf("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt);
+    PRINT_INFO("STATE PARAMETERS:\n");
+    PRINT_INFO("\t- gravity_mag: %.4f\n", gravity_mag);
+    PRINT_INFO("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag);
+    PRINT_INFO("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt);
     assert(state_options.num_cameras == (int)camera_fisheye.size());
     for (int n = 0; n < state_options.num_cameras; n++) {
-      std::cout << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl;
-      std::cout << "cam_" << n << "_wh:" << endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl;
-      std::cout << "cam_" << n << "_intrinsic(0:3):" << endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl;
-      std::cout << "cam_" << n << "_intrinsic(4:7):" << endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl;
-      std::cout << "cam_" << n << "_extrinsic(0:3):" << endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl;
-      std::cout << "cam_" << n << "_extrinsic(4:6):" << endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl;
+      std::stringstream ss;
+      ss << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl;
+      ss << "cam_" << n << "_wh:" << endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl;
+      ss << "cam_" << n << "_intrinsic(0:3):" << endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl;
+      ss << "cam_" << n << "_intrinsic(4:7):" << endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl;
+      ss << "cam_" << n << "_extrinsic(0:3):" << endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl;
+      ss << "cam_" << n << "_extrinsic(4:6):" << endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl;
       Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity();
       T_CtoI.block(0, 0, 3, 3) = quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose();
       T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * camera_extrinsics.at(n).block(4, 0, 3, 1);
-      std::cout << "T_C" << n << "toI:" << endl << T_CtoI << std::endl << std::endl;
+      ss << "T_C" << n << "toI:" << endl << T_CtoI << std::endl << std::endl;
+      PRINT_INFO(ss.str().c_str());
     }
   }
 
@@ -235,20 +239,20 @@ struct VioManagerOptions {
    * @brief This function will print out all parameters releated to our visual trackers.
    */
   void print_trackers() {
-    printf("FEATURE TRACKING PARAMETERS:\n");
-    printf("\t- use_klt: %d\n", use_klt);
-    printf("\t- use_stereo: %d\n", use_stereo);
-    printf("\t- use_aruco: %d\n", use_aruco);
-    printf("\t- downsize aruco: %d\n", downsize_aruco);
-    printf("\t- downsize cameras: %d\n", downsample_cameras);
-    printf("\t- use multi-threading: %d\n", use_multi_threading);
-    printf("\t- num_pts: %d\n", num_pts);
-    printf("\t- fast threshold: %d\n", fast_threshold);
-    printf("\t- grid X by Y: %d by %d\n", grid_x, grid_y);
-    printf("\t- min px dist: %d\n", min_px_dist);
-    printf("\t- hist method: %d\n", (int)histogram_method);
-    printf("\t- knn ratio: %.3f\n", knn_ratio);
-    printf("\t- use mask?: %d\n", use_mask);
+    PRINT_INFO("FEATURE TRACKING PARAMETERS:\n");
+    PRINT_INFO("\t- use_klt: %d\n", use_klt);
+    PRINT_INFO("\t- use_stereo: %d\n", use_stereo);
+    PRINT_INFO("\t- use_aruco: %d\n", use_aruco);
+    PRINT_INFO("\t- downsize aruco: %d\n", downsize_aruco);
+    PRINT_INFO("\t- downsize cameras: %d\n", downsample_cameras);
+    PRINT_INFO("\t- use multi-threading: %d\n", use_multi_threading);
+    PRINT_INFO("\t- num_pts: %d\n", num_pts);
+    PRINT_INFO("\t- fast threshold: %d\n", fast_threshold);
+    PRINT_INFO("\t- grid X by Y: %d by %d\n", grid_x, grid_y);
+    PRINT_INFO("\t- min px dist: %d\n", min_px_dist);
+    PRINT_INFO("\t- hist method: %d\n", (int)histogram_method);
+    PRINT_INFO("\t- knn ratio: %.3f\n", knn_ratio);
+    PRINT_INFO("\t- use mask?: %d\n", use_mask);
     featinit_options.print();
   }
 
@@ -285,14 +289,14 @@ struct VioManagerOptions {
    * This allows for visual checking that everything was loaded properly from ROS/CMD parsers.
    */
   void print_simulation() {
-    printf("SIMULATION PARAMETERS:\n");
-    printf(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init);
-    printf(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb);
-    printf(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements);
-    printf("\t- traj path: %s\n", sim_traj_path.c_str());
-    printf("\t- dist thresh: %.2f\n", sim_distance_threshold);
-    printf("\t- cam feq: %.2f\n", sim_freq_cam);
-    printf("\t- imu feq: %.2f\n", sim_freq_imu);
+    PRINT_INFO("SIMULATION PARAMETERS:\n");
+    PRINT_INFO(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init);
+    PRINT_INFO(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb);
+    PRINT_INFO(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements);
+    PRINT_INFO("\t- traj path: %s\n", sim_traj_path.c_str());
+    PRINT_INFO("\t- dist thresh: %.2f\n", sim_distance_threshold);
+    PRINT_INFO("\t- cam feq: %.2f\n", sim_freq_cam);
+    PRINT_INFO("\t- imu feq: %.2f\n", sim_freq_imu);
   }
 };
 
diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp
index 43753b619..952be7fee 100644
--- a/ov_msckf/src/ros_serial_msckf.cpp
+++ b/ov_msckf/src/ros_serial_msckf.cpp
@@ -91,7 +91,7 @@ int main(int argc, char **argv) {
   if (nh.hasParam("path_gt")) {
     std::string path_to_gt;
     nh.param<std::string>("path_gt", path_to_gt, "");
-    if(!path_to_gt.empty()) {
+    if (!path_to_gt.empty()) {
       DatasetReader::load_gt_file(path_to_gt, gt_states);
       ROS_INFO("gt file path is: %s", path_to_gt.c_str());
     }
diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp
index ff827bda2..6c481475a 100644
--- a/ov_msckf/src/run_simulation.cpp
+++ b/ov_msckf/src/run_simulation.cpp
@@ -27,6 +27,7 @@
 #include "utils/colors.h"
 #include "utils/dataset_reader.h"
 #include "utils/parse_cmd.h"
+#include "utils/print.h"
 #include "utils/sensor_data.h"
 
 #if ROS_AVAILABLE
@@ -74,8 +75,8 @@ int main(int argc, char **argv) {
   Eigen::Matrix<double, 17, 1> imustate;
   bool success = sim->get_state(sim->current_timestamp(), imustate);
   if (!success) {
-    printf(RED "[SIM]: Could not initialize the filter to the first state\n" RESET);
-    printf(RED "[SIM]: Did the simulator load properly???\n" RESET);
+    PRINT_ERROR(RED "[SIM]: Could not initialize the filter to the first state\n" RESET);
+    PRINT_ERROR(RED "[SIM]: Did the simulator load properly???\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp
index ff7596cf8..01d7d04f7 100644
--- a/ov_msckf/src/sim/Simulator.cpp
+++ b/ov_msckf/src/sim/Simulator.cpp
@@ -21,6 +21,8 @@
 
 #include "Simulator.h"
 
+#include <sstream>
+
 using namespace ov_msckf;
 
 Simulator::Simulator(VioManagerOptions &params_) {
@@ -29,9 +31,9 @@ Simulator::Simulator(VioManagerOptions &params_) {
   //===============================================================
 
   // Nice startup message
-  printf("=======================================\n");
-  printf("VISUAL-INERTIAL SIMULATOR STARTING\n");
-  printf("=======================================\n");
+  PRINT_DEBUG("=======================================\n");
+  PRINT_DEBUG("VISUAL-INERTIAL SIMULATOR STARTING\n");
+  PRINT_DEBUG("=======================================\n");
 
   // Store a copy of our params
   this->params = params_;
@@ -43,8 +45,9 @@ Simulator::Simulator(VioManagerOptions &params_) {
 
   // Check that the max cameras matches the size of our cam matrices
   if (params.state_options.num_cameras != (int)params.camera_fisheye.size()) {
-    printf(RED "[SIM]: camera calib size does not match max cameras...\n" RESET);
-    printf(RED "[SIM]: got %d but expected %d max cameras\n" RESET, (int)params.camera_fisheye.size(), params.state_options.num_cameras);
+    PRINT_ERROR(RED "[SIM]: camera calib size does not match max cameras...\n" RESET);
+    PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras\n" RESET, (int)params.camera_fisheye.size(),
+                params.state_options.num_cameras);
     std::exit(EXIT_FAILURE);
   }
 
@@ -62,7 +65,7 @@ Simulator::Simulator(VioManagerOptions &params_) {
   Eigen::Vector3d p_IinG_init;
   bool success_pose_init = spline.get_pose(timestamp, R_GtoI_init, p_IinG_init);
   if (!success_pose_init) {
-    printf(RED "[SIM]: unable to find the first pose in the spline...\n" RESET);
+    PRINT_ERROR(RED "[SIM]: unable to find the first pose in the spline...\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -78,7 +81,7 @@ Simulator::Simulator(VioManagerOptions &params_) {
 
     // Check if it fails
     if (!success_pose) {
-      printf(RED "[SIM]: unable to find jolt in the groundtruth data to initialize at\n" RESET);
+      PRINT_ERROR(RED "[SIM]: unable to find jolt in the groundtruth data to initialize at\n" RESET);
       std::exit(EXIT_FAILURE);
     }
 
@@ -95,7 +98,7 @@ Simulator::Simulator(VioManagerOptions &params_) {
       timestamp_last_cam += 1.0 / params.sim_freq_cam;
     }
   }
-  printf("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline.get_start_time());
+  PRINT_DEBUG("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline.get_start_time());
 
   // Append the current true bias to our history
   hist_true_bias_time.push_back(timestamp_last_imu - 1.0 / params.sim_freq_imu);
@@ -170,7 +173,7 @@ Simulator::Simulator(VioManagerOptions &params_) {
   // double dt = 0.25/freq_cam;
   double dt = 0.25;
   size_t mapsize = featmap.size();
-  printf("[SIM]: Generating map features at %d rate\n", (int)(1.0 / dt));
+  PRINT_DEBUG("[SIM]: Generating map features at %d rate\n", (int)(1.0 / dt));
 
   // Loop through each camera
   // NOTE: we loop through cameras here so that the feature map for camera 1 will always be the same
@@ -204,8 +207,8 @@ Simulator::Simulator(VioManagerOptions &params_) {
     }
 
     // Debug print
-    printf("[SIM]: Generated %d map features in total over %d frames (camera %d)\n", (int)(featmap.size() - mapsize),
-           (int)((time_synth - spline.get_start_time()) / dt), i);
+    PRINT_DEBUG("[SIM]: Generated %d map features in total over %d frames (camera %d)\n", (int)(featmap.size() - mapsize),
+                (int)((time_synth - spline.get_start_time()) / dt), i);
     mapsize = featmap.size();
   }
 
@@ -349,8 +352,8 @@ bool Simulator::get_next_cam(double &time_cam, std::vector<int> &camids,
 
     // If we do not have enough, generate more
     if ((int)uvs.size() < params.num_pts) {
-      printf(YELLOW "[SIM]: cam %d was unable to generate enough features (%d < %d projections)\n" RESET, (int)i, (int)uvs.size(),
-             params.num_pts);
+      PRINT_WARNING(YELLOW "[SIM]: cam %d was unable to generate enough features (%d < %d projections)\n" RESET, (int)i, (int)uvs.size(),
+                    params.num_pts);
     }
 
     // If greater than only select the first set
@@ -386,14 +389,14 @@ void Simulator::load_data(std::string path_traj) {
   std::ifstream file;
   file.open(path_traj);
   if (!file) {
-    printf(RED "ERROR: Unable to open simulation trajectory file...\n" RESET);
-    printf(RED "ERROR: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "ERROR: Unable to open simulation trajectory file...\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 
   // Debug print
   std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1);
-  printf("[SIM]: loaded trajectory %s\n", base_filename.c_str());
+  PRINT_DEBUG("[SIM]: loaded trajectory %s\n", base_filename.c_str());
 
   // Loop through each line of this file
   std::string current_line;
@@ -422,7 +425,10 @@ void Simulator::load_data(std::string path_traj) {
     // Only a valid line if we have all the parameters
     if (i > 7) {
       traj_data.push_back(data);
-      // std::cout << std::setprecision(15) << data.transpose() << std::endl;
+
+      // std::stringstream ss;
+      // ss << std::setprecision(15) << data.transpose() << std::endl;
+      // PRINT_DEBUG(ss.str().c_str());
     }
   }
 
@@ -431,8 +437,8 @@ void Simulator::load_data(std::string path_traj) {
 
   // Error if we don't have any data
   if (traj_data.empty()) {
-    printf(RED "ERROR: Could not parse any data from the file!!\n" RESET);
-    printf(RED "ERROR: %s\n" RESET, path_traj.c_str());
+    PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, path_traj.c_str());
     std::exit(EXIT_FAILURE);
   }
 }
diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp
index b06170475..863e076dd 100644
--- a/ov_msckf/src/state/Propagator.cpp
+++ b/ov_msckf/src/state/Propagator.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "Propagator.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 using namespace ov_msckf;
@@ -29,14 +30,14 @@ void Propagator::propagate_and_clone(std::shared_ptr<State> state, double timest
   // If the difference between the current update time and state is zero
   // We should crash, as this means we would have two clones at the same time!!!!
   if (state->_timestamp == timestamp) {
-    printf(RED "Propagator::propagate_and_clone(): Propagation called again at same timestep at last update timestep!!!!\n" RESET);
+    PRINT_ERROR(RED "Propagator::propagate_and_clone(): Propagation called again at same timestep at last update timestep!!!!\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
   // We should crash if we are trying to propagate backwards
   if (state->_timestamp > timestamp) {
-    printf(RED "Propagator::propagate_and_clone(): Propagation called trying to propagate backwards in time!!!!\n" RESET);
-    printf(RED "Propagator::propagate_and_clone(): desired propagation = %.4f\n" RESET, (timestamp - state->_timestamp));
+    PRINT_ERROR(RED "Propagator::propagate_and_clone(): Propagation called trying to propagate backwards in time!!!!\n" RESET);
+    PRINT_ERROR(RED "Propagator::propagate_and_clone(): desired propagation = %.4f\n" RESET, (timestamp - state->_timestamp));
     std::exit(EXIT_FAILURE);
   }
 
@@ -187,7 +188,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
   // Ensure we have some measurements in the first place!
   if (imu_data.empty()) {
     if (warn)
-      printf(YELLOW "Propagator::select_imu_readings(): No IMU measurements. IMU-CAMERA are likely messed up!!!\n" RESET);
+      PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): No IMU measurements. IMU-CAMERA are likely messed up!!!\n" RESET);
     return prop_data;
   }
 
@@ -202,7 +203,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
     if (imu_data.at(i + 1).timestamp > time0 && imu_data.at(i).timestamp < time0) {
       ov_core::ImuData data = Propagator::interpolate_data(imu_data.at(i), imu_data.at(i + 1), time0);
       prop_data.push_back(data);
-      // printf("propagation #%d = CASE 1 = %.3f => %.3f\n",
+      // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n",
       // (int)i,data.timestamp-prop_data.at(0).timestamp,time0-prop_data.at(0).timestamp);
       continue;
     }
@@ -212,7 +213,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
     // Then we should just append the whole measurement time to our propagation vector
     if (imu_data.at(i).timestamp >= time0 && imu_data.at(i + 1).timestamp <= time1) {
       prop_data.push_back(imu_data.at(i));
-      // printf("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp);
+      // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp);
       continue;
     }
 
@@ -234,11 +235,11 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
       } else if (imu_data.at(i).timestamp > time1) {
         ov_core::ImuData data = interpolate_data(imu_data.at(i - 1), imu_data.at(i), time1);
         prop_data.push_back(data);
-        // printf("propagation #%d = CASE 3.1 = %.3f => %.3f\n",
+        // PRINT_DEBUG("propagation #%d = CASE 3.1 = %.3f => %.3f\n",
         // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0);
       } else {
         prop_data.push_back(imu_data.at(i));
-        // printf("propagation #%d = CASE 3.2 = %.3f => %.3f\n",
+        // PRINT_DEBUG("propagation #%d = CASE 3.2 = %.3f => %.3f\n",
         // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0);
       }
       // If the added IMU message doesn't end exactly at the camera time
@@ -246,7 +247,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
       if (prop_data.at(prop_data.size() - 1).timestamp != time1) {
         ov_core::ImuData data = interpolate_data(imu_data.at(i), imu_data.at(i + 1), time1);
         prop_data.push_back(data);
-        // printf("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0);
+        // PRINT_DEBUG("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0);
       }
       break;
     }
@@ -255,7 +256,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
   // Check that we have at least one measurement to propagate with
   if (prop_data.empty()) {
     if (warn)
-      printf(
+      PRINT_WARNING(
           YELLOW
           "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET,
           (int)prop_data.size());
@@ -265,8 +266,8 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
   // If we did not reach the whole integration period (i.e., the last inertial measurement we have is smaller then the time we want to
   // reach) Then we should just "stretch" the last measurement to be the whole period (case 3 in the above loop)
   // if(time1-imu_data.at(imu_data.size()-1).timestamp > 1e-3) {
-  //    printf(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing). IMU-CAMERA are
-  //    likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data;
+  //    PRINT_DEBUG(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing).
+  //    IMU-CAMERA are likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data;
   //}
 
   // Loop through and ensure we do not have an zero dt values
@@ -274,8 +275,8 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
   for (size_t i = 0; i < prop_data.size() - 1; i++) {
     if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12) {
       if (warn)
-        printf(YELLOW "Propagator::select_imu_readings(): Zero DT between IMU reading %d and %d, removing it!\n" RESET, (int)i,
-               (int)(i + 1));
+        PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): Zero DT between IMU reading %d and %d, removing it!\n" RESET, (int)i,
+                      (int)(i + 1));
       prop_data.erase(prop_data.begin() + i);
       i--;
     }
@@ -284,7 +285,7 @@ std::vector<ov_core::ImuData> Propagator::select_imu_readings(const std::vector<
   // Check that we have at least one measurement to propagate with
   if (prop_data.size() < 2) {
     if (warn)
-      printf(
+      PRINT_WARNING(
           YELLOW
           "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET,
           (int)prop_data.size());
diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h
index 77a85ca5b..2fac854ac 100644
--- a/ov_msckf/src/state/Propagator.h
+++ b/ov_msckf/src/state/Propagator.h
@@ -23,6 +23,7 @@
 #define OV_MSCKF_STATE_PROPAGATOR_H
 
 #include "state/StateHelper.h"
+#include "utils/print.h"
 #include "utils/quat_ops.h"
 #include "utils/sensor_data.h"
 
@@ -71,10 +72,10 @@ class Propagator {
 
     /// Nice print function of what parameters we have loaded
     void print() {
-      printf("\t- gyroscope_noise_density: %.6f\n", sigma_w);
-      printf("\t- accelerometer_noise_density: %.5f\n", sigma_a);
-      printf("\t- gyroscope_random_walk: %.7f\n", sigma_wb);
-      printf("\t- accelerometer_random_walk: %.6f\n", sigma_ab);
+      PRINT_INFO("\t- gyroscope_noise_density: %.6f\n", sigma_w);
+      PRINT_INFO("\t- accelerometer_noise_density: %.5f\n", sigma_a);
+      PRINT_INFO("\t- gyroscope_random_walk: %.7f\n", sigma_wb);
+      PRINT_INFO("\t- accelerometer_random_walk: %.6f\n", sigma_ab);
     }
   };
 
@@ -170,7 +171,7 @@ class Propagator {
   static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) {
     // time-distance lambda
     double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp);
-    // cout << "lambda - " << lambda << endl;
+    // PRINT_DEBUG("lambda - %d\n", lambda);
     // interpolate between the two times
     ov_core::ImuData data;
     data.timestamp = timestamp;
diff --git a/ov_msckf/src/state/StateHelper.cpp b/ov_msckf/src/state/StateHelper.cpp
index 15570a4bc..7ad16ecfd 100644
--- a/ov_msckf/src/state/StateHelper.cpp
+++ b/ov_msckf/src/state/StateHelper.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "StateHelper.h"
+#include "utils/print.h"
 
 using namespace ov_core;
 using namespace ov_msckf;
@@ -30,7 +31,7 @@ void StateHelper::EKFPropagation(std::shared_ptr<State> state, const std::vector
 
   // We need at least one old and new variable
   if (order_NEW.empty() || order_OLD.empty()) {
-    printf(RED "StateHelper::EKFPropagation() - Called with empty variable arrays!\n" RESET);
+    PRINT_ERROR(RED "StateHelper::EKFPropagation() - Called with empty variable arrays!\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -38,9 +39,9 @@ void StateHelper::EKFPropagation(std::shared_ptr<State> state, const std::vector
   int size_order_NEW = order_NEW.at(0)->size();
   for (size_t i = 0; i < order_NEW.size() - 1; i++) {
     if (order_NEW.at(i)->id() + order_NEW.at(i)->size() != order_NEW.at(i + 1)->id()) {
-      printf(RED "StateHelper::EKFPropagation() - Called with non-contiguous state elements!\n" RESET);
-      printf(RED
-             "StateHelper::EKFPropagation() - This code only support a state transition which is in the same order as the state\n" RESET);
+      PRINT_ERROR(RED "StateHelper::EKFPropagation() - Called with non-contiguous state elements!\n" RESET);
+      PRINT_ERROR(
+          RED "StateHelper::EKFPropagation() - This code only support a state transition which is in the same order as the state\n" RESET);
       std::exit(EXIT_FAILURE);
     }
     size_order_NEW += order_NEW.at(i + 1)->size();
@@ -95,7 +96,7 @@ void StateHelper::EKFPropagation(std::shared_ptr<State> state, const std::vector
   bool found_neg = false;
   for (int i = 0; i < diags.rows(); i++) {
     if (diags(i) < 0.0) {
-      printf(RED "StateHelper::EKFPropagation() - diagonal at %d is %.2f\n" RESET, i, diags(i));
+      PRINT_WARNING(RED "StateHelper::EKFPropagation() - diagonal at %d is %.2f\n" RESET, i, diags(i));
       found_neg = true;
     }
   }
@@ -162,7 +163,7 @@ void StateHelper::EKFUpdate(std::shared_ptr<State> state, const std::vector<std:
   bool found_neg = false;
   for (int i = 0; i < diags.rows(); i++) {
     if (diags(i) < 0.0) {
-      printf(RED "StateHelper::EKFUpdate() - diagonal at %d is %.2f\n" RESET, i, diags(i));
+      PRINT_WARNING(RED "StateHelper::EKFUpdate() - diagonal at %d is %.2f\n" RESET, i, diags(i));
       found_neg = true;
     }
   }
@@ -245,8 +246,8 @@ void StateHelper::marginalize(std::shared_ptr<State> state, std::shared_ptr<Type
 
   // Check if the current state has the element we want to marginalize
   if (std::find(state->_variables.begin(), state->_variables.end(), marg) == state->_variables.end()) {
-    printf(RED "StateHelper::marginalize() - Called on variable that is not in the state\n" RESET);
-    printf(RED "StateHelper::marginalize() - Marginalization, does NOT work on sub-variables yet...\n" RESET);
+    PRINT_ERROR(RED "StateHelper::marginalize() - Called on variable that is not in the state\n" RESET);
+    PRINT_ERROR(RED "StateHelper::marginalize() - Marginalization, does NOT work on sub-variables yet...\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -353,8 +354,8 @@ std::shared_ptr<Type> StateHelper::clone(std::shared_ptr<State> state, std::shar
 
   // Check if the current state has this variable
   if (new_clone == nullptr) {
-    printf(RED "StateHelper::clone() - Called on variable is not in the state\n" RESET);
-    printf(RED "StateHelper::clone() - Ensure that the variable specified is a variable, or sub-variable..\n" RESET);
+    PRINT_ERROR(RED "StateHelper::clone() - Called on variable is not in the state\n" RESET);
+    PRINT_ERROR(RED "StateHelper::clone() - Ensure that the variable specified is a variable, or sub-variable..\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -369,8 +370,8 @@ bool StateHelper::initialize(std::shared_ptr<State> state, std::shared_ptr<Type>
 
   // Check that this new variable is not already initialized
   if (std::find(state->_variables.begin(), state->_variables.end(), new_variable) != state->_variables.end()) {
-    std::cerr << "StateHelper::initialize() - Called on variable that is already in the state" << std::endl;
-    std::cerr << "StateHelper::initialize() - Found this variable at " << new_variable->id() << " in covariance" << std::endl;
+    PRINT_ERROR("StateHelper::initialize_invertible() - Called on variable that is already in the state\n");
+    PRINT_ERROR("StateHelper::initialize_invertible() - Found this variable at %d in covariance\n", new_variable->id());
     std::exit(EXIT_FAILURE);
   }
 
@@ -381,12 +382,12 @@ bool StateHelper::initialize(std::shared_ptr<State> state, std::shared_ptr<Type>
   for (int r = 0; r < R.rows(); r++) {
     for (int c = 0; c < R.cols(); c++) {
       if (r == c && R(0, 0) != R(r, c)) {
-        printf(RED "StateHelper::initialize() - Your noise is not isotropic!\n" RESET);
-        printf(RED "StateHelper::initialize() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0));
+        PRINT_ERROR(RED "StateHelper::initialize() - Your noise is not isotropic!\n" RESET);
+        PRINT_ERROR(RED "StateHelper::initialize() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0));
         std::exit(EXIT_FAILURE);
       } else if (r != c && R(r, c) != 0.0) {
-        printf(RED "StateHelper::initialize() - Your noise is not diagonal!\n" RESET);
-        printf(RED "StateHelper::initialize() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c);
+        PRINT_ERROR(RED "StateHelper::initialize() - Your noise is not diagonal!\n" RESET);
+        PRINT_ERROR(RED "StateHelper::initialize() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c);
         std::exit(EXIT_FAILURE);
       }
     }
@@ -460,8 +461,8 @@ void StateHelper::initialize_invertible(std::shared_ptr<State> state, std::share
 
   // Check that this new variable is not already initialized
   if (std::find(state->_variables.begin(), state->_variables.end(), new_variable) != state->_variables.end()) {
-    std::cerr << "StateHelper::initialize_invertible() - Called on variable that is already in the state" << std::endl;
-    std::cerr << "StateHelper::initialize_invertible() - Found this variable at " << new_variable->id() << " in covariance" << std::endl;
+    PRINT_ERROR("StateHelper::initialize_invertible() - Called on variable that is already in the state\n");
+    PRINT_ERROR("StateHelper::initialize_invertible() - Found this variable at %d in covariance\n", new_variable->id());
     std::exit(EXIT_FAILURE);
   }
 
@@ -472,12 +473,12 @@ void StateHelper::initialize_invertible(std::shared_ptr<State> state, std::share
   for (int r = 0; r < R.rows(); r++) {
     for (int c = 0; c < R.cols(); c++) {
       if (r == c && R(0, 0) != R(r, c)) {
-        printf(RED "StateHelper::initialize_invertible() - Your noise is not isotropic!\n" RESET);
-        printf(RED "StateHelper::initialize_invertible() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0));
+        PRINT_ERROR(RED "StateHelper::initialize_invertible() - Your noise is not isotropic!\n" RESET);
+        PRINT_ERROR(RED "StateHelper::initialize_invertible() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0));
         std::exit(EXIT_FAILURE);
       } else if (r != c && R(r, c) != 0.0) {
-        printf(RED "StateHelper::initialize_invertible() - Your noise is not diagonal!\n" RESET);
-        printf(RED "StateHelper::initialize_invertible() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c);
+        PRINT_ERROR(RED "StateHelper::initialize_invertible() - Your noise is not diagonal!\n" RESET);
+        PRINT_ERROR(RED "StateHelper::initialize_invertible() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c);
         std::exit(EXIT_FAILURE);
       }
     }
@@ -543,14 +544,17 @@ void StateHelper::initialize_invertible(std::shared_ptr<State> state, std::share
   // Now collect results, and add it to the state variables
   new_variable->set_local_id(oldSize);
   state->_variables.push_back(new_variable);
-  // std::cout << new_variable->id() <<  " init dx = " << (H_Linv * res).transpose() << std::endl;
+
+  // std::stringstream ss;
+  // ss << new_variable->id() <<  " init dx = " << (H_Linv * res).transpose() << std::endl;
+  // PRINT_DEBUG(ss.str().c_str());
 }
 
 void StateHelper::augment_clone(std::shared_ptr<State> state, Eigen::Matrix<double, 3, 1> last_w) {
 
   // We can't insert a clone that occured at the same timestamp!
   if (state->_clones_IMU.find(state->_timestamp) != state->_clones_IMU.end()) {
-    printf(RED "TRIED TO INSERT A CLONE AT THE SAME TIME AS AN EXISTING CLONE, EXITING!#!@#!@#\n" RESET);
+    PRINT_ERROR(RED "TRIED TO INSERT A CLONE AT THE SAME TIME AS AN EXISTING CLONE, EXITING!#!@#!@#\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -561,7 +565,7 @@ void StateHelper::augment_clone(std::shared_ptr<State> state, Eigen::Matrix<doub
   // Cast to a JPL pose type, check if valid
   std::shared_ptr<PoseJPL> pose = std::dynamic_pointer_cast<PoseJPL>(posetemp);
   if (pose == nullptr) {
-    printf(RED "INVALID OBJECT RETURNED FROM STATEHELPER CLONE, EXITING!#!@#!@#\n" RESET);
+    PRINT_ERROR(RED "INVALID OBJECT RETURNED FROM STATEHELPER CLONE, EXITING!#!@#!@#\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h
index 248691c39..c22dd7ca0 100644
--- a/ov_msckf/src/state/StateOptions.h
+++ b/ov_msckf/src/state/StateOptions.h
@@ -23,6 +23,8 @@
 #define OV_MSCKF_STATE_OPTIONS_H
 
 #include "types/LandmarkRepresentation.h"
+#include "utils/print.h"
+
 #include <climits>
 
 using namespace ov_type;
@@ -81,21 +83,21 @@ struct StateOptions {
 
   /// Nice print function of what parameters we have loaded
   void print() {
-    printf("\t- use_fej: %d\n", do_fej);
-    printf("\t- use_imuavg: %d\n", imu_avg);
-    printf("\t- use_rk4int: %d\n", use_rk4_integration);
-    printf("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose);
-    printf("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics);
-    printf("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset);
-    printf("\t- max_clones: %d\n", max_clone_size);
-    printf("\t- max_slam: %d\n", max_slam_features);
-    printf("\t- max_slam_in_update: %d\n", max_slam_in_update);
-    printf("\t- max_msckf_in_update: %d\n", max_msckf_in_update);
-    printf("\t- max_aruco: %d\n", max_aruco_features);
-    printf("\t- max_cameras: %d\n", num_cameras);
-    printf("\t- feat_rep_msckf: %s\n", LandmarkRepresentation::as_string(feat_rep_msckf).c_str());
-    printf("\t- feat_rep_slam: %s\n", LandmarkRepresentation::as_string(feat_rep_slam).c_str());
-    printf("\t- feat_rep_aruco: %s\n", LandmarkRepresentation::as_string(feat_rep_aruco).c_str());
+    PRINT_INFO("\t- use_fej: %d\n", do_fej);
+    PRINT_INFO("\t- use_imuavg: %d\n", imu_avg);
+    PRINT_INFO("\t- use_rk4int: %d\n", use_rk4_integration);
+    PRINT_INFO("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose);
+    PRINT_INFO("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics);
+    PRINT_INFO("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset);
+    PRINT_INFO("\t- max_clones: %d\n", max_clone_size);
+    PRINT_INFO("\t- max_slam: %d\n", max_slam_features);
+    PRINT_INFO("\t- max_slam_in_update: %d\n", max_slam_in_update);
+    PRINT_INFO("\t- max_msckf_in_update: %d\n", max_msckf_in_update);
+    PRINT_INFO("\t- max_aruco: %d\n", max_aruco_features);
+    PRINT_INFO("\t- max_cameras: %d\n", num_cameras);
+    PRINT_INFO("\t- feat_rep_msckf: %s\n", LandmarkRepresentation::as_string(feat_rep_msckf).c_str());
+    PRINT_INFO("\t- feat_rep_slam: %s\n", LandmarkRepresentation::as_string(feat_rep_slam).c_str());
+    PRINT_INFO("\t- feat_rep_aruco: %s\n", LandmarkRepresentation::as_string(feat_rep_aruco).c_str());
   }
 };
 
diff --git a/ov_msckf/src/test_sim_meas.cpp b/ov_msckf/src/test_sim_meas.cpp
index f5c1fbea3..415cda43b 100644
--- a/ov_msckf/src/test_sim_meas.cpp
+++ b/ov_msckf/src/test_sim_meas.cpp
@@ -69,8 +69,7 @@ int main(int argc, char **argv) {
     Eigen::Vector3d wm, am;
     bool hasimu = sim.get_next_imu(time_imu, wm, am);
     if (hasimu) {
-      cout << "new imu measurement = " << std::setprecision(15) << time_imu << std::setprecision(3) << " | w = " << wm.norm()
-           << " | a = " << am.norm() << endl;
+      PRINT_DEBUG("new imu measurement = %0.15g | w = %0.3g | a = %0.3g\n", time_imu, wm.norm(), am.norm());
     }
 
     // CAM: get the next simulated camera uv measurements if we have them
@@ -79,11 +78,10 @@ int main(int argc, char **argv) {
     std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>> feats;
     bool hascam = sim.get_next_cam(time_cam, camids, feats);
     if (hascam) {
-      cout << "new cam measurement = " << std::setprecision(15) << time_cam;
-      cout << std::setprecision(3) << " | " << camids.size() << " cameras | uvs(0) = " << feats.at(0).size() << std::endl;
+      PRINT_DEBUG("new cam measurement = %0.15g | %u cameras | uvs(0) = %u \n", time_cam, camids.size(), feats.at(0).size());
     }
   }
 
   // Done!
   return EXIT_SUCCESS;
-}
+};
\ No newline at end of file
diff --git a/ov_msckf/src/test_sim_repeat.cpp b/ov_msckf/src/test_sim_repeat.cpp
index d2b35bd48..8084058f5 100644
--- a/ov_msckf/src/test_sim_repeat.cpp
+++ b/ov_msckf/src/test_sim_repeat.cpp
@@ -37,6 +37,7 @@
 #include "sim/Simulator.h"
 #include "utils/parse_cmd.h"
 #include "utils/parse_ros.h"
+#include "utils/print.h"
 
 using namespace ov_msckf;
 
@@ -146,6 +147,6 @@ int main(int argc, char **argv) {
   }
 
   // Done!
-  printf("success! they all are the same!\n");
+  PRINT_INFO("success! they all are the same!\n");
   return EXIT_SUCCESS;
 }
diff --git a/ov_msckf/src/update/UpdaterMSCKF.cpp b/ov_msckf/src/update/UpdaterMSCKF.cpp
index 4449e1051..d4e53c1fc 100644
--- a/ov_msckf/src/update/UpdaterMSCKF.cpp
+++ b/ov_msckf/src/update/UpdaterMSCKF.cpp
@@ -20,6 +20,10 @@
  */
 
 #include "UpdaterMSCKF.h"
+#include "utils/print.h"
+
+#include <iostream>
+#include <sstream>
 
 using namespace ov_core;
 using namespace ov_msckf;
@@ -187,16 +191,18 @@ void UpdaterMSCKF::update(std::shared_ptr<State> state, std::vector<std::shared_
     } else {
       boost::math::chi_squared chi_squared_dist(res.rows());
       chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
-      printf(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
+      PRINT_WARNING(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
     }
 
     // Check if we should delete or not
     if (chi2 > _options.chi2_multipler * chi2_check) {
       (*it2)->to_delete = true;
       it2 = feature_vec.erase(it2);
-      // cout << "featid = " << feat.featid << endl;
-      // cout << "chi2 = " << chi2 << " > " << _options.chi2_multipler*chi2_check << endl;
-      // cout << "res = " << endl << res.transpose() << endl;
+      // PRINT_DEBUG("featid = %d\n", feat.featid);
+      // PRINT_DEBUG("chi2 = %f > %f\n", chi2, _options.chi2_multipler*chi2_check);
+      // std::stringstream ss;
+      // ss << "res = " << std::endl << res.transpose() << std::endl;
+      // PRINT_DEBUG(ss.str().c_str());
       continue;
     }
 
@@ -253,10 +259,10 @@ void UpdaterMSCKF::update(std::shared_ptr<State> state, std::vector<std::shared_
   rT5 = boost::posix_time::microsec_clock::local_time();
 
   // Debug print timing information
-  // printf("[MSCKF-UP]: %.4f seconds to clean\n",(rT1-rT0).total_microseconds() * 1e-6);
-  // printf("[MSCKF-UP]: %.4f seconds to triangulate\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[MSCKF-UP]: %.4f seconds create system (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6, (int)feature_vec.size());
-  // printf("[MSCKF-UP]: %.4f seconds compress system\n",(rT4-rT3).total_microseconds() * 1e-6);
-  // printf("[MSCKF-UP]: %.4f seconds update state (%d size)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)res_big.rows());
-  // printf("[MSCKF-UP]: %.4f seconds total\n",(rT5-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[MSCKF-UP]: %.4f seconds to clean\n",(rT1-rT0).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[MSCKF-UP]: %.4f seconds to triangulate\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[MSCKF-UP]: %.4f seconds create system (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6, (int)feature_vec.size());
+  // PRINT_DEBUG("[MSCKF-UP]: %.4f seconds compress system\n",(rT4-rT3).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[MSCKF-UP]: %.4f seconds update state (%d size)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)res_big.rows());
+  // PRINT_DEBUG("[MSCKF-UP]: %.4f seconds total\n",(rT5-rT1).total_microseconds() * 1e-6);
 }
diff --git a/ov_msckf/src/update/UpdaterOptions.h b/ov_msckf/src/update/UpdaterOptions.h
index 792a08785..8a8c87278 100644
--- a/ov_msckf/src/update/UpdaterOptions.h
+++ b/ov_msckf/src/update/UpdaterOptions.h
@@ -22,6 +22,8 @@
 #ifndef OV_MSCKF_UPDATER_OPTIONS_H
 #define OV_MSCKF_UPDATER_OPTIONS_H
 
+#include "utils/print.h"
+
 namespace ov_msckf {
 
 /**
@@ -40,8 +42,8 @@ struct UpdaterOptions {
 
   /// Nice print function of what parameters we have loaded
   void print() {
-    printf("\t- chi2_multipler: %.1f\n", chi2_multipler);
-    printf("\t- sigma_pix: %.2f\n", sigma_pix);
+    PRINT_INFO("\t- chi2_multipler: %.1f\n", chi2_multipler);
+    PRINT_INFO("\t- sigma_pix: %.2f\n", sigma_pix);
   }
 };
 
diff --git a/ov_msckf/src/update/UpdaterSLAM.cpp b/ov_msckf/src/update/UpdaterSLAM.cpp
index 17a0bb880..aae9178bd 100644
--- a/ov_msckf/src/update/UpdaterSLAM.cpp
+++ b/ov_msckf/src/update/UpdaterSLAM.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "UpdaterSLAM.h"
+#include "utils/print.h"
 
 #include <memory>
 
@@ -211,10 +212,10 @@ void UpdaterSLAM::delayed_init(std::shared_ptr<State> state, std::vector<std::sh
 
   // Debug print timing information
   // if(!feature_vec.empty()) {
-  //    printf("[SLAM-DELAY]: %.4f seconds to clean\n",(rT1-rT0).total_microseconds() * 1e-6);
-  //    printf("[SLAM-DELAY]: %.4f seconds to triangulate\n",(rT2-rT1).total_microseconds() * 1e-6);
-  //    printf("[SLAM-DELAY]: %.4f seconds initialize (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6, (int)feature_vec.size());
-  //    printf("[SLAM-DELAY]: %.4f seconds total\n",(rT3-rT1).total_microseconds() * 1e-6);
+  //    PRINT_DEBUG("[SLAM-DELAY]: %.4f seconds to clean\n",(rT1-rT0).total_microseconds() * 1e-6);
+  //    PRINT_DEBUG("[SLAM-DELAY]: %.4f seconds to triangulate\n",(rT2-rT1).total_microseconds() * 1e-6);
+  //    PRINT_DEBUG("[SLAM-DELAY]: %.4f seconds initialize (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6, (int)feature_vec.size());
+  //    PRINT_DEBUG("[SLAM-DELAY]: %.4f seconds total\n",(rT3-rT1).total_microseconds() * 1e-6);
   //}
 }
 
@@ -369,7 +370,7 @@ void UpdaterSLAM::update(std::shared_ptr<State> state, std::vector<std::shared_p
     } else {
       boost::math::chi_squared chi_squared_dist(res.rows());
       chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
-      printf(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
+      PRINT_WARNING(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
     }
 
     // Check if we should delete or not
@@ -377,8 +378,8 @@ void UpdaterSLAM::update(std::shared_ptr<State> state, std::vector<std::shared_p
         ((int)feat.featid < state->_options.max_aruco_features) ? _options_aruco.chi2_multipler : _options_slam.chi2_multipler;
     if (chi2 > chi2_multipler * chi2_check) {
       if ((int)feat.featid < state->_options.max_aruco_features)
-        printf(YELLOW "[SLAM-UP]: rejecting aruco tag %d for chi2 thresh (%.3f > %.3f)\n" RESET, (int)feat.featid, chi2,
-               chi2_multipler * chi2_check);
+        PRINT_WARNING(YELLOW "[SLAM-UP]: rejecting aruco tag %d for chi2 thresh (%.3f > %.3f)\n" RESET, (int)feat.featid, chi2,
+                      chi2_multipler * chi2_check);
       (*it2)->to_delete = true;
       it2 = feature_vec.erase(it2);
       continue;
@@ -386,7 +387,7 @@ void UpdaterSLAM::update(std::shared_ptr<State> state, std::vector<std::shared_p
 
     // Debug print when we are going to update the aruco tags
     if ((int)feat.featid < state->_options.max_aruco_features)
-      printf("[SLAM-UP]: accepted aruco tag %d for chi2 thresh (%.3f < %.3f)\n", (int)feat.featid, chi2, chi2_multipler * chi2_check);
+      PRINT_DEBUG("[SLAM-UP]: accepted aruco tag %d for chi2 thresh (%.3f < %.3f)\n", (int)feat.featid, chi2, chi2_multipler * chi2_check);
 
     // We are good!!! Append to our large H vector
     size_t ct_hx = 0;
@@ -435,10 +436,10 @@ void UpdaterSLAM::update(std::shared_ptr<State> state, std::vector<std::shared_p
   rT3 = boost::posix_time::microsec_clock::local_time();
 
   // Debug print timing information
-  // printf("[SLAM-UP]: %.4f seconds to clean\n",(rT1-rT0).total_microseconds() * 1e-6);
-  // printf("[SLAM-UP]: %.4f seconds creating linear system\n",(rT2-rT1).total_microseconds() * 1e-6);
-  // printf("[SLAM-UP]: %.4f seconds to update (%d feats of %d size)\n",(rT3-rT2).total_microseconds() * 1e-6, (int)feature_vec.size(),
-  // (int)Hx_big.rows()); printf("[SLAM-UP]: %.4f seconds total\n",(rT3-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[SLAM-UP]: %.4f seconds to clean\n",(rT1-rT0).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[SLAM-UP]: %.4f seconds creating linear system\n",(rT2-rT1).total_microseconds() * 1e-6);
+  // PRINT_DEBUG("[SLAM-UP]: %.4f seconds to update (%d feats of %d size)\n",(rT3-rT2).total_microseconds() * 1e-6, (int)feature_vec.size(),
+  // (int)Hx_big.rows()); PRINT_DEBUG("[SLAM-UP]: %.4f seconds total\n",(rT3-rT1).total_microseconds() * 1e-6);
 }
 
 void UpdaterSLAM::change_anchors(std::shared_ptr<State> state) {
diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp
index 704ba7989..3b4a0bdbc 100644
--- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp
+++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp
@@ -20,6 +20,7 @@
  */
 
 #include "UpdaterZeroVelocity.h"
+#include "utils/print.h"
 
 using namespace ov_msckf;
 
@@ -62,7 +63,7 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
 
   // Check that we have at least one measurement to propagate with
   if (imu_recent.size() < 2) {
-    printf(RED "[ZUPT]: There are no IMU data to check for zero velocity with!!\n" RESET);
+    PRINT_WARNING(RED "[ZUPT]: There are no IMU data to check for zero velocity with!!\n" RESET);
     last_zupt_state_timestamp = 0.0;
     return false;
   }
@@ -159,7 +160,7 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
   } else {
     boost::math::chi_squared chi_squared_dist(res.rows());
     chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
-    printf(YELLOW "[ZUPT]: chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
+    PRINT_WARNING(YELLOW "[ZUPT]: chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
   }
 
   // Check if the image disparity
@@ -202,10 +203,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
     }
     disparity_passed = (average_disparity < _zupt_max_disparity && num_features > 20);
     if (disparity_passed) {
-      printf(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, (int)num_features);
+      PRINT_WARNING(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity,
+                    (int)num_features);
     } else {
-      printf(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity,
-             (int)num_features);
+      PRINT_INFO(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity,
+                 (int)num_features);
     }
   }
 
@@ -213,12 +215,12 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
   // We need to pass the chi2 and not be above our velocity threshold
   if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) {
     last_zupt_state_timestamp = 0.0;
-    printf(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2,
-           _options.chi2_multipler * chi2_check);
+    PRINT_WARNING(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2,
+                  _options.chi2_multipler * chi2_check);
     return false;
   }
-  printf(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2,
-         _options.chi2_multipler * chi2_check);
+  PRINT_DEBUG(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2,
+              _options.chi2_multipler * chi2_check);
 
   // Do our update, only do this update if we have previously detected
   // If we have succeeded, then we should remove the current timestamp feature tracks
diff --git a/ov_msckf/src/utils/parse_cmd.h b/ov_msckf/src/utils/parse_cmd.h
index 8699fb84e..0f72b8f9e 100644
--- a/ov_msckf/src/utils/parse_cmd.h
+++ b/ov_msckf/src/utils/parse_cmd.h
@@ -24,6 +24,7 @@
 
 #include "core/VioManagerOptions.h"
 #include "utils/CLI11.hpp"
+#include "utils/print.h"
 
 namespace ov_msckf {
 
@@ -180,20 +181,20 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) {
   if (params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN ||
       params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN ||
       params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) {
-    printf(RED "VioManager(): invalid feature representation specified:\n" RESET);
-    printf(RED "\t- GLOBAL_3D\n" RESET);
-    printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
-    printf(RED "\t- ANCHORED_3D\n" RESET);
-    printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
-    printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
-    printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
+    PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET);
+    PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET);
+    PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_3D\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
   // Enforce that we have enough cameras to run
   if (params.state_options.num_cameras < 1) {
-    printf(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET);
-    printf(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras);
+    PRINT_ERROR(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET);
+    PRINT_ERROR(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras);
     std::exit(EXIT_FAILURE);
   }
 
@@ -205,10 +206,10 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) {
   } else if (histogram_method_str == "CLAHE") {
     params.histogram_method = TrackBase::CLAHE;
   } else {
-    printf(RED "VioManager(): invalid feature histogram specified:\n" RESET);
-    printf(RED "\t- NONE\n" RESET);
-    printf(RED "\t- HISTOGRAM\n" RESET);
-    printf(RED "\t- CLAHE\n" RESET);
+    PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET);
+    PRINT_ERROR(RED "\t- NONE\n" RESET);
+    PRINT_ERROR(RED "\t- HISTOGRAM\n" RESET);
+    PRINT_ERROR(RED "\t- CLAHE\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
diff --git a/ov_msckf/src/utils/parse_ros.h b/ov_msckf/src/utils/parse_ros.h
index 1bfcac890..8913f8955 100644
--- a/ov_msckf/src/utils/parse_ros.h
+++ b/ov_msckf/src/utils/parse_ros.h
@@ -26,6 +26,7 @@
 #include <ros/ros.h>
 
 #include "core/VioManagerOptions.h"
+#include "utils/print.h"
 
 namespace ov_msckf {
 
@@ -59,8 +60,8 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) {
 
   // Enforce that we have enough cameras to run
   if (params.state_options.num_cameras < 1) {
-    printf(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET);
-    printf(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras);
+    PRINT_ERROR(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET);
+    PRINT_ERROR(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras);
     std::exit(EXIT_FAILURE);
   }
 
@@ -82,13 +83,13 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) {
   if (params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN ||
       params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN ||
       params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) {
-    printf(RED "VioManager(): invalid feature representation specified:\n" RESET);
-    printf(RED "\t- GLOBAL_3D\n" RESET);
-    printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
-    printf(RED "\t- ANCHORED_3D\n" RESET);
-    printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
-    printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
-    printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
+    PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET);
+    PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET);
+    PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_3D\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
+    PRINT_ERROR(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -160,10 +161,10 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) {
   } else if (histogram_method_str == "CLAHE") {
     params.histogram_method = TrackBase::CLAHE;
   } else {
-    printf(RED "VioManager(): invalid feature histogram specified:\n" RESET);
-    printf(RED "\t- NONE\n" RESET);
-    printf(RED "\t- HISTOGRAM\n" RESET);
-    printf(RED "\t- CLAHE\n" RESET);
+    PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET);
+    PRINT_ERROR(RED "\t- NONE\n" RESET);
+    PRINT_ERROR(RED "\t- HISTOGRAM\n" RESET);
+    PRINT_ERROR(RED "\t- CLAHE\n" RESET);
     std::exit(EXIT_FAILURE);
   }
 
@@ -174,9 +175,9 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) {
     nh.param<std::string>("mask" + std::to_string(i), mask_path, "");
     if (params.use_mask) {
       if (!boost::filesystem::exists(mask_path)) {
-        printf(RED "VioManager(): invalid mask path:\n" RESET);
-        printf(RED "\t- mask%d\n" RESET, i);
-        printf(RED "\t- %s\n" RESET, mask_path.c_str());
+        PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET);
+        PRINT_ERROR(RED "\t- mask%d\n" RESET, i);
+        PRINT_ERROR(RED "\t- %s\n" RESET, mask_path.c_str());
         std::exit(EXIT_FAILURE);
       }
       params.masks.insert({i, cv::imread(mask_path, cv::IMREAD_GRAYSCALE)});