Result image path can be configured

config/calib.yaml

@@ -2,7 +2,8 @@
 common:
     image_file: "/home/ycj/data/calib/image/0.png"
     pcd_file: "/home/ycj/data/calib/pcd/0.pcd"
-    result_file: "/home/ycj/data/calib/extrinsic.txt"
+    result_file: "extrinsic.txt"
+    result_dir: "/home/ycj/data/calib/"
 
 # Camera Parameters. Adjust them!
 camera:

config/multi_calib.yaml

@@ -1,8 +1,11 @@
 # Data path. adjust them!
 common:
-    image_path:  "/home/ycj/data/calib/image/old"
-    pcd_path:  "/home/ycj/data/calib/pcd/old"
-    result_path:  "/home/ycj/data/calib/extrinsic.txt"
+    image_path:  "/home/ycj/data/calib/image/old/"
+    image_ext: ".bmp"
+    pcd_path:  "/home/ycj/data/calib/pcd/old/"
+    result_dir: "/home/ycj/data/calib/"
+    result_file:  "extrinsic.txt" # saved in the result_dir specified above
+    numdigits: 1 # number of digits, from now the dataset indexes are zero padded
     data_num: 3
 # Camera Parameters. Adjust them!
 camera:

src/lidar_camera_calib.cpp

@@ -12,6 +12,7 @@ using namespace std;
 string image_file;
 string pcd_file;
 string result_file;
+string result_dir;
 
 // Camera config
 vector<double> camera_matrix;
@@ -196,7 +197,11 @@ int main(int argc, char **argv) {
   nh.param<string>("common/image_file", image_file, "");
   nh.param<string>("common/pcd_file", pcd_file, "");
   nh.param<string>("common/result_file", result_file, "");
-  std::cout << "pcd_file path:" << pcd_file << std::endl;
+  nh.param<string>("common/result_dir", result_dir, "");
+  result_file = result_dir + result_file;
+  string ImgOutName =result_file;
+  ImgOutName.resize(ImgOutName.size()-4);
+
   nh.param<vector<double>>("camera/camera_matrix", camera_matrix,
                            vector<double>());
   nh.param<vector<double>>("camera/dist_coeffs", dist_coeffs, vector<double>());
@@ -252,9 +257,10 @@ int main(int argc, char **argv) {
   pcl::toROSMsg(*rgb_cloud, pub_cloud);
   pub_cloud.header.frame_id = "livox";
   calibra.init_rgb_cloud_pub_.publish(pub_cloud);
+
   cv::Mat init_img = calibra.getProjectionImg(calib_params);
   cv::imshow("Initial extrinsic", init_img);
-  cv::imwrite("/home/ycj/data/calib/init.png", init_img);
+  cv::imwrite(ImgOutName+"_init.png", init_img);
   cv::waitKey(1000);
 
   if (use_rough_calib) {
@@ -381,7 +387,7 @@ int main(int argc, char **argv) {
   outfile << 0 << "," << 0 << "," << 0 << "," << 1 << std::endl;
   cv::Mat opt_img = calibra.getProjectionImg(calib_params);
   cv::imshow("Optimization result", opt_img);
-  cv::imwrite("/home/ycj/data/calib/opt.png", opt_img);
+  cv::imwrite(ImgOutName+"_opt.png", opt_img);
   cv::waitKey(1000);
   Eigen::Matrix3d init_rotation;
   init_rotation << 0, -1.0, 0, 0, 0, -1.0, 1, 0, 0;

src/lidar_camera_multi_calib.cpp

@@ -10,9 +10,12 @@ using namespace std;
 
 // Data path
 string image_path;
+string image_ext;
 string pcd_path;
-string result_path;
+string result_file;
+string result_dir;
 int data_num;
+int numdigits;
 
 // Camera config
 vector<double> camera_matrix;
@@ -190,15 +193,29 @@ void roughCalib(std::vector<Calibration> &calibs, Vector6d &calib_params,
     }
 }
 
+std::string zeropad(int number, int digitnum){
+  std::string old_str = std::to_string(number);
+  size_t n = size_t(digitnum);
+  int precision = n - std::min(n, old_str.size());
+  std::string new_str = std::string(precision, '0').append(old_str);
+  return new_str;
+}
+
 int main(int argc, char **argv) {
   ros::init(argc, argv, "lidarCamCalib");
   ros::NodeHandle nh;
   ros::Rate loop_rate(0.1);
 
   nh.param<string>("common/image_path", image_path, "");
+  nh.param<string>("common/image_ext", image_ext, "");
   nh.param<string>("common/pcd_path", pcd_path, "");
-  nh.param<string>("common/result_path", result_path, "");
+  nh.param<string>("common/result_file", result_file, "");
+  nh.param<string>("common/result_dir", result_dir, "");
+  boost::filesystem::create_directories(result_dir);
+  result_file = result_dir + result_file;
+
   nh.param<int>("common/data_num", data_num, 1);
+  nh.param<int>("common/numdigits", numdigits, 1);
   nh.param<vector<double>>("camera/camera_matrix", camera_matrix,
                            vector<double>());
   nh.param<vector<double>>("camera/dist_coeffs", dist_coeffs, vector<double>());
@@ -208,8 +225,9 @@ int main(int argc, char **argv) {
   std::vector<Calibration> calibs;
   for (size_t i = 0; i < data_num; i++) {
     string image_file, pcd_file = "";
-    image_file = image_path + "/" + std::to_string(i) + ".bmp";
-    pcd_file = pcd_path + "/" + std::to_string(i) + ".pcd";
+    image_file = image_path + zeropad(i,numdigits) + image_ext;
+    std::cout<<"\t"<<i<<"/"<<data_num<<" "<<image_file<<"\n";
+    pcd_file = pcd_path + zeropad(i,numdigits) + ".pcd";
     Calibration single_calib(image_file, pcd_file, calib_config_file);
     single_calib.fx_ = camera_matrix[0];
     single_calib.cx_ = camera_matrix[2];
@@ -254,15 +272,21 @@ int main(int argc, char **argv) {
   calib_params[3] = T[0];
   calib_params[4] = T[1];
   calib_params[5] = T[2];
-  cv::Mat init_img = calibs[0].getProjectionImg(calib_params);
-  cv::imshow("Initial extrinsic", init_img);
-  cv::waitKey(1000);
+  for (size_t i = 0; i < data_num; i++) {
+    cv::Mat init_img = calibs[i].getProjectionImg(calib_params);
+    cv::imshow("Initial extrinsic "+zeropad(i,numdigits), init_img);
+    cv::imwrite(result_dir+zeropad(i,numdigits)+"_init.png", init_img);
+    cv::waitKey(1000);
+  }
   if (use_rough_calib) {
     roughCalib(calibs, calib_params, DEG2RAD(0.2), 40);
   }
-  cv::Mat test_img = calibs[0].getProjectionImg(calib_params);
-  cv::imshow("After rough extrinsic", test_img);
-  cv::waitKey(1000);
+  for (size_t i = 0; i < data_num; i++) {
+    cv::Mat test_img = calibs[i].getProjectionImg(calib_params);
+    cv::imshow("After rough extrinsic "+zeropad(i,numdigits), test_img);
+    cv::imwrite(result_dir+zeropad(i,numdigits)+"_rough.png", test_img);
+    cv::waitKey(1000);
+  }
   int iter = 0;
   // Maximum match distance threshold: 15 pixels
   // If initial extrinsic lead to error over 15 pixels, the algorithm will not
@@ -369,15 +393,18 @@ int main(int argc, char **argv) {
   R = Eigen::AngleAxisd(calib_params[0], Eigen::Vector3d::UnitZ()) *
       Eigen::AngleAxisd(calib_params[1], Eigen::Vector3d::UnitY()) *
       Eigen::AngleAxisd(calib_params[2], Eigen::Vector3d::UnitX());
-  std::ofstream outfile(result_path);
+  std::ofstream outfile(result_file);
   for (int i = 0; i < 3; i++) {
     outfile << R(i, 0) << "," << R(i, 1) << "," << R(i, 2) << "," << T[i]
             << std::endl;
   }
   outfile << 0 << "," << 0 << "," << 0 << "," << 1 << std::endl;
-  cv::Mat opt_img = calibs[0].getProjectionImg(calib_params);
-  cv::imshow("Optimization result", opt_img);
-  cv::waitKey(1000);
+  for (size_t i = 0; i < data_num; i++) {
+    cv::Mat opt_img = calibs[i].getProjectionImg(calib_params);
+    cv::imshow("Optimization result "+zeropad(i,numdigits), opt_img);
+    cv::imwrite(result_dir+zeropad(i,numdigits)+"_opt.png", opt_img);
+    cv::waitKey(1000);
+  }
   Eigen::Matrix3d init_rotation;
   init_rotation << 0, -1.0, 0, 0, 0, -1.0, 1, 0, 0;
   Eigen::Matrix3d adjust_rotation;