diff --git a/config/calib.yaml b/config/calib.yaml index c33f269..552d40f 100644 --- a/config/calib.yaml +++ b/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: diff --git a/config/multi_calib.yaml b/config/multi_calib.yaml index 6fe447d..55e2981 100644 --- a/config/multi_calib.yaml +++ b/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: diff --git a/src/lidar_camera_calib.cpp b/src/lidar_camera_calib.cpp index a801a0c..e1ec9ce 100644 --- a/src/lidar_camera_calib.cpp +++ b/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 camera_matrix; @@ -196,7 +197,11 @@ int main(int argc, char **argv) { nh.param("common/image_file", image_file, ""); nh.param("common/pcd_file", pcd_file, ""); nh.param("common/result_file", result_file, ""); - std::cout << "pcd_file path:" << pcd_file << std::endl; + nh.param("common/result_dir", result_dir, ""); + result_file = result_dir + result_file; + string ImgOutName =result_file; + ImgOutName.resize(ImgOutName.size()-4); + nh.param>("camera/camera_matrix", camera_matrix, vector()); nh.param>("camera/dist_coeffs", dist_coeffs, vector()); @@ -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; diff --git a/src/lidar_camera_multi_calib.cpp b/src/lidar_camera_multi_calib.cpp index c10403b..891ecb6 100644 --- a/src/lidar_camera_multi_calib.cpp +++ b/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 camera_matrix; @@ -190,15 +193,29 @@ void roughCalib(std::vector &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("common/image_path", image_path, ""); + nh.param("common/image_ext", image_ext, ""); nh.param("common/pcd_path", pcd_path, ""); - nh.param("common/result_path", result_path, ""); + nh.param("common/result_file", result_file, ""); + nh.param("common/result_dir", result_dir, ""); + boost::filesystem::create_directories(result_dir); + result_file = result_dir + result_file; + nh.param("common/data_num", data_num, 1); + nh.param("common/numdigits", numdigits, 1); nh.param>("camera/camera_matrix", camera_matrix, vector()); nh.param>("camera/dist_coeffs", dist_coeffs, vector()); @@ -208,8 +225,9 @@ int main(int argc, char **argv) { std::vector 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"<