diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 59cb3846..1e15f785 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -7,7 +7,6 @@ set(SOURCES lidar_odometry_gui.cpp lidar_odometry_utils.h lidar_odometry_utils.cpp lidar_odometry_utils_optimizers.cpp lidar_odometry.cpp lidar_odometry.h - lidar_odometry_gui_utils.cpp lidar_odometry_gui_utils.h toml.hpp toml_io.h toml_io.cpp "../../core/src/utils.cpp" ) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 28e366bf..0875bc9b 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -132,14 +132,12 @@ bool load_data(std::vector& input_file_names, const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); - const int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); - std::size_t minSize = std::min(laz_files.size(), csv_files.size()); if (!preloadedCalibration.empty()) { std::cout << "Loaded calibration for:\n"; for (const auto& [sn, _] : preloadedCalibration) - std::cout << " -> " << sn << "\n\n"; + std::cout << " -> " << sn << "\n"; bool hasError = false; @@ -197,27 +195,12 @@ bool load_data(std::vector& input_file_names, }*/ } - // --- Working directories - fs::path wdp = fs::path(working_directory) / "preview"; - if (!fs::exists(wdp)) - { - std::cout << "Creating folder: " << wdp << "\n"; - fs::create_directory(wdp); - } - params.working_directory_preview = wdp.string(); - - fs::path wdp_cache = fs::path(working_directory) / "cache"; - if (!fs::exists(wdp_cache)) - { - std::cout << "Creating folder: " << wdp_cache << "\n"; - fs::create_directory(wdp_cache); - } - params.working_directory_cache = wdp_cache.string(); - + const int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); // --- Load IMU data (still sequential, could parallelize later) std::cout << "loading IMU data from 'imu****.csv' using ID " << imuNumberToUse - << " from reference '" << sn_file << "' ..\n\n"; + << " from reference '" << sn_file << "' ..\n"; + std::size_t minSize = std::min(laz_files.size(), csv_files.size()); // Each thread loads into its own local vector std::vector, FusionVector, FusionVector>>> imuChunks(minSize); @@ -254,6 +237,13 @@ bool load_data(std::vector& input_file_names, pointsPerFile.resize(minSize); + // --- Working directories + + fs::path wdp = fs::path(working_directory) / "preview"; + params.working_directory_preview = wdp.string(); + fs::path wdp_cache = fs::path(working_directory) / "cache"; + params.working_directory_cache = wdp_cache.string(); + // --- Parallel load of LAZ files tbb::parallel_for(size_t(0), minSize, [&](size_t i) { @@ -267,6 +257,12 @@ bool load_data(std::vector& input_file_names, // Optional calibration validation (first file only) if ((i == 0) && params.save_calibration_validation) { + if (!fs::exists(wdp)) + { + std::cout << "Creating folder: " << wdp << "\n"; + fs::create_directory(wdp); + } + fs::path outFile = wdp / "calibrationValidation.asc"; std::ofstream testPointcloud(outFile); if (!testPointcloud) @@ -500,6 +496,14 @@ bool compute_step_1( worker_data.clear(); worker_data.reserve(n_iter); + std::filesystem::path wdp_cache = params.working_directory_cache; + + if (!fs::exists(wdp_cache)) + { + std::cout << "Creating folder: " << wdp_cache << "\n"; + fs::create_directory(wdp_cache); + } + for (size_t i = 0; i < n_iter; i++) { if (i % 50 == 0) @@ -598,14 +602,9 @@ bool compute_step_1( if (params.decimation > 0.0 && points.size() > 1000) intermediate_points = decimate(points, params.decimation, params.decimation, params.decimation); - std::filesystem::path path = params.working_directory_cache; - std::string original_points_cache_filename = "original_points_" + std::to_string(worker_data.size()) + ".cache"; - wd.original_points_cache_file_name = path / original_points_cache_filename; + wd.original_points_cache_file_name = wdp_cache / original_points_cache_filename; - // std::cout << "original_points_cache_file_name " << path.string() << std::endl; - // wd.save_points(filtered_points, wd.original_points_cache_file_name); - // wd.original_points = filtered_points; if (!save_vector_data(wd.original_points_cache_file_name.string(), points)) { std::cout << "problem with save_vector_data file '" << wd.original_points_cache_file_name.string() << "'" << std::endl; @@ -613,10 +612,7 @@ bool compute_step_1( } std::string intermediate_points_cache_filename = "intermediate_points_" + std::to_string(worker_data.size()) + ".cache"; - wd.intermediate_points_cache_file_name = path / intermediate_points_cache_filename; - - // remove "flooding" messages from console. this also interfered with previous message of "running iterations: i/total" - // std::cout << "intermediate_points_cache_file_name " << path2.string() << std::endl; + wd.intermediate_points_cache_file_name = wdp_cache / intermediate_points_cache_filename; if (intermediate_points.size() > 0) { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 05bc6311..68541134 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -9,7 +9,6 @@ #include #include -#include "lidar_odometry_gui_utils.h" #include "lidar_odometry_utils.h" #include "lidar_odometry.h" #include diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui_utils.cpp deleted file mode 100644 index f43cc3c0..00000000 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui_utils.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include "lidar_odometry_gui_utils.h" -#include -#include - - -void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd) -{ - Eigen::LLT> cholSolver(covar); - Eigen::Matrix3d transform = cholSolver.matrixL(); - - const double pi = 3.141592; - const double di = 0.02; - const double dj = 0.04; - const double du = di * 2 * pi; - const double dv = dj * pi; - glColor3f(color.x(), color.y(), color.z()); - - for (double i = 0; i < 1.0; i += di) // horizonal - { - for (double j = 0; j < 1.0; j += dj) // vertical - { - double u = i * 2 * pi; // 0 to 2pi - double v = (j - 0.5) * pi; //-pi/2 to pi/2 - - const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); - const Eigen::Vector3d pp1(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)); - const Eigen::Vector3d pp2(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)); - const Eigen::Vector3d pp3(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)); - Eigen::Vector3d tp0 = transform * (nstd * pp0) + mean; - Eigen::Vector3d tp1 = transform * (nstd * pp1) + mean; - Eigen::Vector3d tp2 = transform * (nstd * pp2) + mean; - Eigen::Vector3d tp3 = transform * (nstd * pp3) + mean; - - glBegin(GL_LINE_LOOP); - glVertex3dv(tp0.data()); - glVertex3dv(tp1.data()); - glVertex3dv(tp2.data()); - glVertex3dv(tp3.data()); - glEnd(); - } - } -} - diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_gui_utils.h deleted file mode 100644 index 7bace15b..00000000 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui_utils.h +++ /dev/null @@ -1,4 +0,0 @@ -#include - -// this function draws ellipse for each bucket -void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd); \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index c06e9cc6..11f92154 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -599,37 +599,68 @@ std::unordered_map MLvxCalib::GetIdToSnMapping(const std::stri return dataMap; } -std::unordered_map MLvxCalib::GetCalibrationFromFile(const std::string &filename) +inline bool JsonGetBool( + const nlohmann::json& obj, + const char* key, + bool defaultValue = false) { - if (!std::filesystem::exists(filename)) + if (!obj.contains(key)) + return defaultValue; + + const auto& v = obj.at(key); + + if (v.is_boolean()) + return v.get(); + + if (v.is_string()) { - return std::unordered_map(); + std::string s = v.get(); + std::transform(s.begin(), s.end(), s.begin(), ::toupper); + return (s == "TRUE" || s == "1" || s == "YES"); } - std::unordered_map dataMap; - std::ifstream file(filename); + return defaultValue; +} + +std::unordered_map MLvxCalib::GetCalibrationFromFile(const std::string &filename) +{ + std::unordered_map dataMap; + std::ifstream file; using json = nlohmann::json; - json jsonData = json::parse(file); + json jsonData; - // Iterate through the JSON object and parse each value into Eigen::Affine3d + file.open(filename); + if (!file) + { + std::cerr << "Cannot open file '" << filename << "'" << std::endl; + return {}; + } + try + { + jsonData = json::parse(file); + } + catch (const json::exception& e) + { + std::cerr << "JSON parsing error in file '" << filename << "': " << e.what() << std::endl; + return {}; + } + + if (!jsonData.contains("calibration")) + return {}; + + // Iterate through the JSON object and parse each value into Eigen::Affine3d for (auto &calibrationEntry : jsonData["calibration"].items()) { const std::string &lidarSn = calibrationEntry.key(); Eigen::Matrix4d value; // std::cout << "lidarSn : " << lidarSn << std::endl; - if (calibrationEntry.value().contains("identity")) + bool identity = JsonGetBool(calibrationEntry.value(),"identity", false); + if (identity) { - std::string identity = calibrationEntry.value()["identity"].get(); - // std::cout << "identity : " << identity << std::endl; - std::transform(identity.begin(), identity.end(), identity.begin(), ::toupper); - if (identity == "TRUE") - { - dataMap[lidarSn] = Eigen::Matrix4d::Identity(); - continue; - continue; - } + dataMap[lidarSn] = Eigen::Matrix4d::Identity(); + continue; } assert(calibrationEntry.value().contains("data")); @@ -640,7 +671,7 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi { for (int j = 0; j < 4; ++j) { - value(i, j) = matrixRawData[i * 4 + j]; // default is column-major order + value(i, j) = matrixRawData[i * 4 + j]; // default is row-major order } } @@ -650,23 +681,12 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi // std::cout << "order : " << order << std::endl; std::transform(order.begin(), order.end(), order.begin(), ::toupper); if (order == "COLUMN") - { - Eigen::Matrix4d valueT = value.transpose(); - value = valueT; - } + value = value.transpose(); } - if (calibrationEntry.value().contains("inverted")) - { - std::string inverted = calibrationEntry.value()["inverted"].get(); - // std::cout << "inverted : " << inverted << std::endl; - std::transform(inverted.begin(), inverted.end(), inverted.begin(), ::toupper); - if (inverted == "TRUE") - { - Eigen::Matrix4d valueI = value.inverse(); - value = valueI; - } - } + bool inverted = JsonGetBool(calibrationEntry.value(),"inverted", false); + if (inverted) + value = value.inverse(); Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); @@ -675,6 +695,7 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi // Insert into the map dataMap[lidarSn] = value; } + // check for blacklisted if (jsonData.contains("blacklist")) { @@ -682,7 +703,9 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi for (const auto &item : blacklist) { std::string blacklistedSn = item.get(); - dataMap[blacklistedSn] = Eigen::Matrix4d::Zero(); + //dataMap[blacklistedSn] = Eigen::Matrix4d::Zero(); + //avoid dealing with zero matrices later on + dataMap.erase(blacklistedSn); } } return dataMap; @@ -690,17 +713,33 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi std::string MLvxCalib::GetImuSnToUse(const std::string &filename) { - if (!std::filesystem::exists(filename)) + std::ifstream file; + using json = nlohmann::json; + json jsonData; + + file.open(filename); + if (!file) { - return ""; + std::cerr << "Cannot open file '" << filename << "'" << std::endl; + return {}; } - std::unordered_map dataMap; - std::ifstream file(filename); - using json = nlohmann::json; - json jsonData = json::parse(file); + try + { + jsonData = json::parse(file); + } + catch (const json::exception& e) + { + std::cerr << "JSON parsing error in file '" << filename << "': " << e.what() << std::endl; + return {}; + } + + if (!jsonData.contains("imuToUse")) + return {}; + if (!jsonData["imuToUse"].is_string()) + return {}; - return jsonData["imuToUse"]; + return jsonData["imuToUse"].get(); } std::unordered_map MLvxCalib::CombineIntoCalibration(const std::unordered_map &idToSn, const std::unordered_map &calibration) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index 53cb3d0d..75eb24fd 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1959,6 +1959,12 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p update_rgd(params.in_out_params_indoor, params.buckets_indoor, pp, params.m_g.translation()); update_rgd(params.in_out_params_outdoor, params.buckets_outdoor, pp, params.m_g.translation()); + if (!fs::exists(params.working_directory_preview)) + { + std::cout << "Creating folder: " << params.working_directory_preview << "\n"; + fs::create_directory(params.working_directory_preview); + } + for (int i = 0; i < worker_data.size(); i++) { std::vector intermediate_points; diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index ca00c883..d7e73477 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -5,8 +5,9 @@ project(mandeye_raw_data_viewer) # Source files set(SOURCES mandeye_raw_data_viewer.cpp + ../lidar_odometry_step_1/toml_io.cpp + ../lidar_odometry_step_1/lidar_odometry.cpp ../lidar_odometry_step_1/lidar_odometry_utils.cpp - ../lidar_odometry_step_1/lidar_odometry_gui_utils.cpp ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp "../../core/src/utils.cpp" ) diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index d674fb0d..006c9013 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -19,10 +19,11 @@ #include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" -#include "../lidar_odometry_step_1/lidar_odometry_gui_utils.h" +#include "../lidar_odometry_step_1/lidar_odometry.h" #include +#include "tbb/tbb.h" #include #include @@ -73,6 +74,7 @@ struct AllData std::vector points_local; std::vector lidar_ids; }; +std::vector all_data; struct ImuData { @@ -88,19 +90,15 @@ struct ImuData std::vector roll; }; ImuData imu_data_plot; -std::vector all_data; std::vector laz_files; -std::vector csv_files; -std::vector sn_files; std::vector photos_files; std::map photo_files_ts; std::string working_directory = ""; -std::string imuSnToUse; std::string working_directory_preview; -double filter_threshold_xy_inner = 0.1; -double filter_threshold_xy_outer = 70.0; +double filter_threshold_xy_inner = 0.0; //no filtering for raw viewing +double filter_threshold_xy_outer = 300.0; //no filtering for raw viewing bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; @@ -111,7 +109,6 @@ int index_rendered_points_local = -1; // std::vector> all_points_local; // std::vector> all_lidar_ids; std::vector indexes_to_filename; -std::vector all_file_names; double ahrs_gain = 0.5; double wx = 1000000.0; double wy = 1000000.0; @@ -764,45 +761,6 @@ std::vector> get_nn() return nn; } -void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd = 3) -{ - - Eigen::LLT> cholSolver(covar); - Eigen::Matrix3d transform = cholSolver.matrixL(); - - const double pi = 3.141592; - const double di = 0.02; - const double dj = 0.04; - const double du = di * 2 * pi; - const double dv = dj * pi; - glColor3f(color.x(), color.y(), color.z()); - - for (double i = 0; i < 1.0; i += di) // horizonal - { - for (double j = 0; j < 1.0; j += dj) // vertical - { - double u = i * 2 * pi; // 0 to 2pi - double v = (j - 0.5) * pi; //-pi/2 to pi/2 - - const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); - const Eigen::Vector3d pp1(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)); - const Eigen::Vector3d pp2(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)); - const Eigen::Vector3d pp3(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)); - Eigen::Vector3d tp0 = transform * (nstd * pp0) + mean; - Eigen::Vector3d tp1 = transform * (nstd * pp1) + mean; - Eigen::Vector3d tp2 = transform * (nstd * pp2) + mean; - Eigen::Vector3d tp3 = transform * (nstd * pp3) + mean; - - glBegin(GL_LINE_LOOP); - glVertex3dv(tp0.data()); - glVertex3dv(tp1.data()); - glVertex3dv(tp2.data()); - glVertex3dv(tp3.data()); - glEnd(); - } - } -} - std::vector> get_mean_cov() { std::vector> mc; @@ -908,30 +866,54 @@ std::vector> get_mean_cov() void loadData() { - if (!is_init) + std::vector input_file_names = mandeye::fd::OpenFileDialog("Load all files", mandeye::fd::All_Filter, true); + + //no files selected, quit loading + if (input_file_names.empty()) return; - laz_files.clear(); - csv_files.clear(); - sn_files.clear(); - all_file_names.clear(); - std::vector input_file_names; - input_file_names = mandeye::fd::OpenFileDialog("Load all files", mandeye::fd::All_Filter, true); + LidarOdometryParams params; //dummy for load_data function + params.save_calibration_validation = false; + params.filter_threshold_xy_inner = filter_threshold_xy_inner; + params.filter_threshold_xy_outer = filter_threshold_xy_outer; + + std::vector> pointsPerFile; + std::vector, FusionVector, FusionVector>> imu_data; + /* + //start loading process std::sort(std::begin(input_file_names), std::end(input_file_names)); + std::string calibrationFile; + + std::cout << "Selected files for open:\n"; + std::for_each(std::begin(input_file_names), std::end(input_file_names), [&](const std::string& fileName) { - if (fileName.ends_with(".laz") || fileName.ends_with(".las")) + std::cout << fileName << "\n"; + + std::string ext = fs::path(fileName).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + + if (ext == ".laz" || ext == ".las") { laz_files.push_back(fileName); - all_file_names.push_back(fileName); } - else if (fileName.ends_with(".csv")) + else if (ext == ".csv") csv_files.push_back(fileName); - else if (fileName.ends_with(".sn")) - sn_files.push_back(fileName); - else if (fileName.ends_with(".jpg")) { + else if ((ext == ".sn") && sn_file.empty()) + { + sn_file = fileName; + std::cout << "Only using first SN file found\n"; + } + else if (ext == ".mjc") + { + if (calibrationFile.empty()) + calibrationFile = fileName; + else + std::cout << "Unexpected calibration file found (" << fileName << "). Ignored.." << std::endl; + } + else if (ext == ".jpg" || ext == ".jpeg") { photos_files.push_back(fileName); // decode filename e.g.: ` cam0_1761264773592270949.jpg` const std::string filename = fs::path(fileName).stem().string(); @@ -955,45 +937,64 @@ void loadData() } }); + std::cout << "\n"; + if (input_file_names.size() > 0 && laz_files.size() == csv_files.size()) { working_directory = fs::path(input_file_names[0]).parent_path().string(); - const auto calibrationFile = (fs::path(working_directory) / "calibration.json").string(); + if (calibrationFile.empty()) + calibrationFile = (fs::path(working_directory) / "calibration.json").string(); //if no calibration file provided, try to load old default from working directory + + // --- Calibration and IMU setup const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); - imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); - std::cout << "imuSnToUse: " << imuSnToUse << std::endl; + const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); + const auto idToSn = MLvxCalib::GetIdToSnMapping(sn_file); + if (!preloadedCalibration.empty()) { - std::cout << "Loaded calibration for: \n"; + std::cout << "Loaded calibration for:\n"; for (const auto& [sn, _] : preloadedCalibration) - std::cout << " -> " << sn << std::endl; + std::cout << " -> " << sn << "\n\n"; + + bool hasError = false; + + for (const auto& [id, sn] : idToSn) + { + if (preloadedCalibration.find(sn) == preloadedCalibration.end()) + { + std::cerr << "WRONG CALIBRATION FILE! THE SERIAL NUMBER SHOULD BE " << sn << "!!!\n"; + hasError = true; + } + } + + if (!hasError && preloadedCalibration.find(imuSnToUse) == preloadedCalibration.end()) + { + std::cerr << "MISSING CALIBRATION FOR imuSnToUse: " << imuSnToUse << "!!!\n"; + std::cerr << "Available serial numbers in calibration file are:\n"; + for (const auto& [snKey, _] : preloadedCalibration) + std::cerr << " - " << snKey << "\n"; + + hasError = true; + } + + if (hasError) + { + std::cerr << "Press ENTER to exit...\n"; + std::cin.get(); + std::exit(EXIT_FAILURE); + } } else { - std::cout << "There is no calibration.json file in folder (check comment in source code) file: " << __FILE__ << " line: " << __LINE__ << std::endl; - // example file for 2x livox"; - /* - { - "calibration" : { - "47MDL9T0020193" : { - "identity" : "true" - }, - "47MDL9S0020300" : - { - "order" : "ROW", - "inverted" : "TRUE", - "data" : [ - 0.999824, 0.00466397, -0.0181595, -0.00425984, - -0.0181478, -0.00254457, -0.999832, -0.151599, - -0.0047094, 0.999986, -0.00245948, -0.146408, - 0, 0, 0, 1 - ] - } - }, - "imuToUse" : "47MDL9T0020193" - }*/ + std::cout << "There is no calibration file in folder!" << std::endl; + std::cout << "IGNORE THIS MESSAGE IF YOU ONLY HAVE 1 LiDAR" << std::endl; + + // example file for 2x LiDAR": } + + std::cout << "imuSnToUse: " << imuSnToUse << std::endl; + std::cout << "Number of found photos: " << photos_files.size() << std::endl; fs::path wdp = fs::path(input_file_names[0]).parent_path(); @@ -1003,20 +1004,13 @@ void loadData() working_directory_preview = wdp.string(); - for (size_t i = 0; i < input_file_names.size(); i++) - std::cout << input_file_names[i] << std::endl; + std::cout << "Loading IMU" << std::endl; - std::cout << "loading imu" << std::endl; std::vector, FusionVector, FusionVector>> imu_data; for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) { const std::string& imufn = csv_files.at(fileNo); - const std::string snFn = (fileNo >= sn_files.size()) ? ("") : (sn_files.at(fileNo)); - - std::cout << "parsing sn file '" << snFn << "'" << std::endl; - - const auto idToSn = MLvxCalib::GetIdToSnMapping(snFn); // GetId of Imu to use int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); std::cout << "imuNumberToUse " << imuNumberToUse << " at: '" << imufn << "'" << std::endl; @@ -1033,14 +1027,13 @@ void loadData() return std::get<0>(a).first < std::get<0>(b).first; }); - std::cout - << "loading points" << std::endl; + std::cout << "Loading points\n"; std::vector> pointsPerFile; pointsPerFile.resize(laz_files.size()); // std::vector> indexesPerFile; std::mutex mtx; - std::cout << "start std::transform" << std::endl; + std::cout << "Start std::transform\n"; std::transform(std::execution::par_unseq, std::begin(laz_files), std::end(laz_files), std::begin(pointsPerFile), [&](const std::string& fn) { @@ -1073,6 +1066,76 @@ void loadData() // }); std::cout << "std::transform finished" << std::endl; + */ + + if (load_data(input_file_names, params, pointsPerFile, imu_data, false)) + { + //clear possible previous data + + all_data.clear(); + all_data.shrink_to_fit(); + + imu_data_plot.timestampLidar.clear(); + imu_data_plot.angX.clear(); + imu_data_plot.angY.clear(); + imu_data_plot.angZ.clear(); + imu_data_plot.accX.clear(); + imu_data_plot.accY.clear(); + imu_data_plot.accZ.clear(); + imu_data_plot.yaw.clear(); + imu_data_plot.pitch.clear(); + imu_data_plot.roll.clear(); + + imu_data_plot.timestampLidar.shrink_to_fit(); + imu_data_plot.angX.shrink_to_fit(); + imu_data_plot.angY.shrink_to_fit(); + imu_data_plot.angZ.shrink_to_fit(); + imu_data_plot.accX.shrink_to_fit(); + imu_data_plot.accY.shrink_to_fit(); + imu_data_plot.accZ.shrink_to_fit(); + imu_data_plot.yaw.shrink_to_fit(); + imu_data_plot.pitch.shrink_to_fit(); + imu_data_plot.roll.shrink_to_fit(); + + photo_files_ts.clear(); + + laz_files.clear(); + + //specific processing for RAW data viewer + + std::sort(input_file_names.begin(), input_file_names.end()); + for (const auto& fileName : input_file_names) + { + std::string ext = fs::path(fileName).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + + if (ext == ".laz" || ext == ".las") + laz_files.push_back(fileName); + else if (ext == ".jpg" || ext == ".jpeg") { + photos_files.push_back(fileName); + // decode filename e.g.: ` cam0_1761264773592270949.jpg` + const std::string filename = fs::path(fileName).stem().string(); + std::string cam_id = filename.substr(0, filename.find("_")); + std::string timestamp = filename.substr(filename.find("_") + 1, filename.size()); + + std::cout << "cam_id: " << cam_id << std::endl; + std::cout << "timestamp: " << timestamp << std::endl; + std::cout << "filename: " << filename << std::endl; + + if (cam_id == "cam0" && !timestamp.empty()) + { + try { + uint64_t ts = std::stoull(timestamp); + photo_files_ts[ts] = fileName; + } + catch (const std::exception& e) { + std::cerr << "Error parsing timestamp from filename: " << filename << " - " << e.what() << std::endl; + } + } + } + } + + //rest of RAW data viewer processing FusionAhrs ahrs; FusionAhrsInitialise(&ahrs); @@ -1177,9 +1240,9 @@ void loadData() for (const auto& pp : pointsPerFile) number_of_points += pp.size(); - std::cout << "number of points: " << number_of_points << std::endl; + std::cout << "Number of points: " << number_of_points << std::endl; - std::cout << "start indexing points" << std::endl; + std::cout << "Start indexing points" << std::endl; // std::vector points_global; std::vector points_local; @@ -1200,7 +1263,7 @@ void loadData() for (size_t i = 0; i < pointsPerFile.size(); i++) { - std::cout << "indexed: " << i + 1 << " of " << pointsPerFile.size() << " files" << std::endl; + std::cout << "Indexed: " << i + 1 << " of " << pointsPerFile.size() << " files\r"; for (const auto& pp : pointsPerFile[i]) { auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), pp.timestamp, @@ -1285,7 +1348,7 @@ void loadData() } } - std::cout << "indexing points finished" << std::endl; + std::cout << "\nIndexing points finished\n\n"; if (all_data.size() > 0) { @@ -1293,21 +1356,11 @@ void loadData() index_rendered_points_local = 0; } } - else - { - std::cout << "please select files correctly" << std::endl; - std::cout << "input_file_names.size(): " << input_file_names.size() << std::endl; - std::cout << "laz_files.size(): " << laz_files.size() << std::endl; - std::cout << "csv_files.size(): " << csv_files.size() << std::endl; - std::cout << "sn_files.size(): " << sn_files.size() << std::endl; - - std::cout << "condition: input_file_names.size() > 0 && laz_files.size() == csv_files.size() && laz_files.size() == sn_files.size() NOT SATISFIED!!!" << std::endl; - } } void imu_data_gui() { - ImGui::Begin("ImuData"); + ImGui::Begin("IMU data"); { if (imu_data_plot.timestampLidar.size() > 0) { @@ -1319,7 +1372,7 @@ void imu_data_gui() if (all_data[index_rendered_points_local].timestamps.size() > 0) annotation = all_data[index_rendered_points_local].timestamps.front().first; } - if (ImPlot::BeginPlot("Imu - acceleration 'm/s^2", ImVec2(-1, 0))) + if (ImPlot::BeginPlot("IMU - acceleration 'm/s^2", ImVec2(-1, 0))) { ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); @@ -1341,7 +1394,7 @@ void imu_data_gui() ImPlot::EndPlot(); } - if (ImPlot::BeginPlot("Imu - AHRS angles [deg]", ImVec2(-1, 0))) + if (ImPlot::BeginPlot("IMU - AHRS angles [deg]", ImVec2(-1, 0))) { ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); @@ -1413,6 +1466,15 @@ void project_gui() } ImGui::InputDouble("AHRS gain", &ahrs_gain); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Parameter for AHRS(Attitude and Heading Reference System) filter"); + ImGui::Text("Controls how strongly the filter corrects its orientation estimate"); + ImGui::Text("using accelerometer and magnetometer data, relative to the gyroscope integration"); + ImGui::EndTooltip(); + } + // ImGui::Checkbox("is_slerp", &is_slerp); ImGui::PopItemWidth(); @@ -1423,7 +1485,7 @@ void project_gui() if (index_rendered_points_local >= all_data.size()) index_rendered_points_local = all_data.size() - 1; - if (all_file_names.size() > 0) + if (laz_files.size() > 0) { if (index_rendered_points_local >= 0 && index_rendered_points_local < indexes_to_filename.size()) { @@ -1761,14 +1823,17 @@ void display() if (ImGui::BeginMainMenuBar()) { - if (ImGui::Button("Load data")) - loadData(); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Select session to open for analyze (Ctrl+O)"); + //if (!is_init) + { + if (ImGui::Button("Load data")) + loadData(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select session to open for analyze (Ctrl+O)"); - ImGui::SameLine(); - ImGui::Dummy(ImVec2(20, 0)); - ImGui::SameLine(); + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + } if (ImGui::BeginMenu("View")) { @@ -1853,7 +1918,7 @@ void display() if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - std::string fn = all_file_names[indexes_to_filename[index_rendered_points_local]]; + std::string fn = laz_files[indexes_to_filename[index_rendered_points_local]]; ImGui::Text(fn.c_str()); ImGui::EndTooltip(); } @@ -1862,7 +1927,7 @@ void display() if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - std::string fn = all_file_names[indexes_to_filename[index_rendered_points_local]]; + std::string fn = laz_files[indexes_to_filename[index_rendered_points_local]]; ImGui::Text(fn.c_str()); ImGui::EndTooltip(); } diff --git a/core/include/pfd_wrapper.hpp b/core/include/pfd_wrapper.hpp index e5c03130..54cc84ee 100644 --- a/core/include/pfd_wrapper.hpp +++ b/core/include/pfd_wrapper.hpp @@ -7,8 +7,8 @@ namespace mandeye::fd{ static std::string lastLocationHint = ".";//"C:\\"; } const std::vector LAS_LAZ_filter = {"LASzip file (*.laz)", "*.laz", "LAS file (*.las)", "*.las", "All files", "*"}; - const std::vector LazFilter = { "LAZ files (*.laz)", "*.laz *.las" }; - const std::vector ImageFilter = { "Image files", "*.png *.jpg *.jpeg *.bmp"}; + const std::vector LazFilter = { "LAS/LAZ files (*.laz)", "*.las *.laz" }; + const std::vector ImageFilter = { "Image files (*.bmp, *.jpg, *.jpeg, *.png)", "*.bmp *.jpg *.jpeg *.png", "All files", "*" }; const std::vector Calibration_filter = { "Mandeye JSON Calibration (*.mjc)", "*.mjc", "Generic JSON (*.json)", "*.json" }; const std::vector Session_filter = { "Mandeye JSON Session (*.mjs)", "*.mjs", "Generic JSON (*.json)", "*.json" }; @@ -17,13 +17,13 @@ namespace mandeye::fd{ const std::vector IniPoses_filter = { "Mandeye REG Initial poses (*.mri)", "*.mri", "Generic REG initial poses (*.reg)", "*.reg" }; const std::vector Poses_filter = { "Mandeye REG Poses (*.mrp)", "*.mrp", "Generic REG poses (*.reg)", "*.reg" }; - const std::vector Resso_filter = { "Resso (*.reg)", "*.reg" }; - const std::vector Dxf_filter = { "dxf (*.dxf)", "*.dxf" }; - const std::vector Csv_filter = { "Csv (*.csv)", "*.csv" }; - const std::vector Toml_filter = {"Toml (*.toml)", "*.toml"}; - const std::vector All_Filter = {"All files", "*.png *.jpg *.jpeg *.bmp *.las *.laz *.json *.dxf *.csv *.sn"}; + const std::vector Resso_filter = { "RESSO (*.reg)", "*.reg" }; + const std::vector Dxf_filter = { "DXF file (*.dxf)", "*.dxf" }; + const std::vector Csv_filter = { "CSV file (*.csv)", "*.csv" }; + const std::vector Toml_filter = {"TOML file (*.toml)", "*.toml"}; + const std::vector All_Filter = {"All supported files (*.bmp, *.csv, *.dxf, *.jpg, *.jpeg, *.json, *.las, *.laz, *.mjc, *.png, *.sn)", "*.bmp *.csv *.dxf *.jpg *.jpeg *.json *.las *.laz *.mjc *.png *.sn", "All files", "*" }; const std::vector json_filter = { "Calibration file (*.json)", "*.json", "All files", "*" }; - const std::vector sn_filter = { "sn file (*.sn)", "*.sn", "All files", "*" }; + const std::vector sn_filter = { "SN file (*.sn)", "*.sn", "All files", "*" }; std::string OpenFileDialogOneFile(const std::string& title, const std::vector&filter); std::vector OpenFileDialog(const std::string& title, const std::vector&filter, bool multiselect); diff --git a/core/include/utils.hpp b/core/include/utils.hpp index 9d925fc7..e41bd9ab 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -49,6 +49,7 @@ extern float mouse_sensitivity; extern bool is_ortho; extern bool lock_z; +void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd); extern bool show_axes; extern ImVec4 bg_color; extern int point_size; diff --git a/core/src/control_points.cpp b/core/src/control_points.cpp index 1845de24..a5316ec8 100644 --- a/core/src/control_points.cpp +++ b/core/src/control_points.cpp @@ -63,7 +63,9 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & } } + ImGui::NewLine(); ImGui::Separator(); + ImGui::NewLine(); int remove_gcp_index = -1; for (int i = 0; i < cps.size(); i++) @@ -77,12 +79,13 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputText("CP name", cps[i].name, IM_ARRAYSIZE(cps[i].name)); + ImGui::InputText(("CP name##" + std::to_string(i)).c_str(), cps[i].name, IM_ARRAYSIZE(cps[i].name)); + ImGui::SameLine(); ImGui::Checkbox(("is z_0##" + std::to_string(i)).c_str(), &cps[i].is_z_0); if (!cps[i].is_z_0) { - ImGui::Text(("sigma [m]:##" + std::to_string(i)).c_str()); + ImGui::Text("sigma [m]:"); ImGui::InputDouble(("X##" + std::to_string(i)).c_str(), &cps[i].sigma_x, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); @@ -95,7 +98,7 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & if (ImGui::IsItemHovered()) ImGui::SetTooltip(zText); - ImGui::Text(("target_global [m]:##" + std::to_string(i)).c_str()); + ImGui::Text("target_global [m]:"); ImGui::InputDouble(("X##t" + std::to_string(i)).c_str(), &cps[i].x_target_global); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); @@ -351,8 +354,8 @@ void ControlPoints::render(const PointClouds &point_clouds_container) glVertex3f(g.x(), g.y(), g.z()); glVertex3f(g.x(), g.y(), g.z()); - glEnd(); + glColor3f(0.0f, 0.3f, 0.6f); glBegin(GL_LINES); glVertex3f(c.x(), c.y(), c.z()); @@ -398,13 +401,12 @@ void ControlPoints::render(const PointClouds &point_clouds_container) glRasterPos3f(cps[i].x_target_global, cps[i].y_target_global, cps[i].z_target_global + 0.1); glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)cps[i].name); - glColor3f(0, 0, 0); glRasterPos3f(cps[i].x_target_global, cps[i].y_target_global, cps[i].z_target_global); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)("CP")); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("CP_" + std::to_string(i)).c_str())); - glColor3f(0, 0, 0); + glColor3f(0.7f, 0.3f, 0.5f); glRasterPos3f(c.x(), c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)("Point assigned to CP")); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("CP_" + std::to_string(i) + ": initial location").c_str())); } return; diff --git a/core/src/ground_control_points.cpp b/core/src/ground_control_points.cpp index 26c4ff90..021ed9fd 100644 --- a/core/src/ground_control_points.cpp +++ b/core/src/ground_control_points.cpp @@ -48,7 +48,9 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) // int picking_mode_index_to_node_outer = -1; } + ImGui::NewLine(); ImGui::Separator(); + ImGui::NewLine(); int remove_gcp_index = -1; for (int i = 0; i < gpcs.size(); i++) @@ -63,7 +65,7 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) ImGui::PushItemWidth(ImGuiNumberWidth); ImGui::InputText(("GCP name##" + std::to_string(i)).c_str(), gpcs[i].name, IM_ARRAYSIZE(gpcs[i].name)); - ImGui::Text(("sigma [m]:##" + std::to_string(i)).c_str()); + ImGui::Text("sigma [m]:"); ImGui::InputDouble(("X##" + std::to_string(i)).c_str(), &gpcs[i].sigma_x, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); @@ -76,7 +78,7 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) if (ImGui::IsItemHovered()) ImGui::SetTooltip(zText); - ImGui::Text(("coordinates [m]:##" + std::to_string(i)).c_str()); + ImGui::Text("coordinates [m]:"); ImGui::InputDouble(("X##c" + std::to_string(i)).c_str(), &gpcs[i].x); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); @@ -276,8 +278,8 @@ void GroundControlPoints::render(const PointClouds &point_clouds_container) glVertex3f(g.x(), g.y(), g.z()); glVertex3f(g.x(), g.y(), g.z() + gpcs[i].lidar_height_above_ground); - glEnd(); + glColor3f(0.0f, 0.3f, 0.6f); glBegin(GL_LINES); glVertex3f(c.x(), c.y(), c.z()); @@ -303,21 +305,19 @@ void GroundControlPoints::render(const PointClouds &point_clouds_container) draw_ellipse(covar, gcp, Eigen::Vector3f(0.5, 0.5, 0.5), 1.0f); } - glColor3f(0, 0, 0); - glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z + gpcs[i].lidar_height_above_ground + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gpcs[i].name); - - glColor3f(0, 0, 0); + glColor3f(0.7f, 0.3f, 0.5f); glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z + gpcs[i].lidar_height_above_ground); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)("LiDAR center")); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("GCP_" + std::to_string(i) + ": LiDAR center").c_str())); - glColor3f(0, 0, 0); glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)("GCP 'plane on the ground'")); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("GCP_" + std::to_string(i) + ": 'plane on the ground'").c_str())); glColor3f(0, 0, 0); + glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z + gpcs[i].lidar_height_above_ground + 0.1); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)gpcs[i].name); + glRasterPos3f(c.x(), c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)("trajectory node assigned to GCP")); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("GCP_" + std::to_string(i) + ": assigned trajectory node").c_str())); } return; diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 343606fc..e6020718 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -476,6 +476,44 @@ bool initGL(int *argc, char **argv, const std::string &winTitle, void (*display) return (glGetError() == GL_NO_ERROR); } +void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd) +{ + Eigen::LLT> cholSolver(covar); + Eigen::Matrix3d transform = cholSolver.matrixL(); + + const double pi = 3.141592; + const double di = 0.02; + const double dj = 0.04; + const double du = di * 2 * pi; + const double dv = dj * pi; + glColor3f(color.x(), color.y(), color.z()); + + for (double i = 0; i < 1.0; i += di) // horizonal + { + for (double j = 0; j < 1.0; j += dj) // vertical + { + double u = i * 2 * pi; // 0 to 2pi + double v = (j - 0.5) * pi; //-pi/2 to pi/2 + + const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); + const Eigen::Vector3d pp1(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)); + const Eigen::Vector3d pp2(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)); + const Eigen::Vector3d pp3(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)); + Eigen::Vector3d tp0 = transform * (nstd * pp0) + mean; + Eigen::Vector3d tp1 = transform * (nstd * pp1) + mean; + Eigen::Vector3d tp2 = transform * (nstd * pp2) + mean; + Eigen::Vector3d tp3 = transform * (nstd * pp3) + mean; + + glBegin(GL_LINE_LOOP); + glVertex3dv(tp0.data()); + glVertex3dv(tp1.data()); + glVertex3dv(tp2.data()); + glVertex3dv(tp3.data()); + glEnd(); + } + } +} + void showAxes() { if (show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes