Optional verbosity modes

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)

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:

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

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);
   }
 };
 

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;
   }
 

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

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;