Raw data viewer update

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"
 )

apps/lidar_odometry_step_1/lidar_odometry.cpp

@@ -132,14 +132,12 @@ bool load_data(std::vector<std::string>& 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<std::string>& 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<std::vector<std::tuple<std::pair<double, double>, FusionVector, FusionVector>>> imuChunks(minSize);
 
@@ -254,6 +237,13 @@ bool load_data(std::vector<std::string>& 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<std::string>& 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,25 +602,17 @@ 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;
                 std::cout << __FILE__ << " " << __LINE__ << std::endl;
             }
 
             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)
             {

apps/lidar_odometry_step_1/lidar_odometry_gui.cpp

@@ -9,7 +9,6 @@
 #include <GL/freeglut.h>
 #include <portable-file-dialogs.h>
 
-#include "lidar_odometry_gui_utils.h"
 #include "lidar_odometry_utils.h"
 #include "lidar_odometry.h"
 #include <registration_plane_feature.h>

apps/lidar_odometry_step_1/lidar_odometry_utils.cpp

@@ -599,37 +599,68 @@ std::unordered_map<int, std::string> MLvxCalib::GetIdToSnMapping(const std::stri
     return dataMap;
 }
 
-std::unordered_map<std::string, Eigen::Affine3d> 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<bool>();
+
+    if (v.is_string())
     {
-        return std::unordered_map<std::string, Eigen::Affine3d>();
+        std::string s = v.get<std::string>();
+        std::transform(s.begin(), s.end(), s.begin(), ::toupper);
+        return (s == "TRUE" || s == "1" || s == "YES");
     }
-    std::unordered_map<std::string, Eigen::Affine3d> dataMap;
-    std::ifstream file(filename);
 
+    return defaultValue;
+}
+
+std::unordered_map<std::string, Eigen::Affine3d> MLvxCalib::GetCalibrationFromFile(const std::string &filename)
+{
+    std::unordered_map<std::string, Eigen::Affine3d> 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::string>();
-            // 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<std::string, Eigen::Affine3d> 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<std::string, Eigen::Affine3d> 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::string>();
-            // 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,32 +695,51 @@ std::unordered_map<std::string, Eigen::Affine3d> MLvxCalib::GetCalibrationFromFi
         //  Insert into the map
         dataMap[lidarSn] = value;
     }
+
     // check for blacklisted
     if (jsonData.contains("blacklist"))
     {
         json blacklist = jsonData["blacklist"];
         for (const auto &item : blacklist)
         {
             std::string blacklistedSn = item.get<std::string>();
-            dataMap[blacklistedSn] = Eigen::Matrix4d::Zero();
+            //dataMap[blacklistedSn] = Eigen::Matrix4d::Zero();
+			//avoid dealing with zero matrices later on
+            dataMap.erase(blacklistedSn);
         }
     }
     return dataMap;
 }
 
 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<std::string, Eigen::Affine3d> 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::string>();
 }
 
 std::unordered_map<int, Eigen::Affine3d> MLvxCalib::CombineIntoCalibration(const std::unordered_map<int, std::string> &idToSn, const std::unordered_map<std::string, Eigen::Affine3d> &calibration)

apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp

@@ -1959,6 +1959,12 @@ bool compute_step_2(std::vector<WorkerData> &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<Point3Di> intermediate_points;

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"
 )