diff --git a/.gitignore b/.gitignore index f8336b5f..165868ea 100644 --- a/.gitignore +++ b/.gitignore @@ -69,3 +69,4 @@ imgui.ini /out/install/x64-Release /out_old/install +/.gitmodules diff --git a/.gitmodules b/.gitmodules index e9ee8084..58d429d3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,6 +22,7 @@ [submodule "3rdparty/imgui"] path = 3rdparty/imgui url = https://github.com/ocornut/imgui.git + branch = docking [submodule "3rdparty/ImGuizmo"] path = 3rdparty/ImGuizmo url = https://github.com/CedricGuillemet/ImGuizmo.git diff --git a/CMakeSettings.json b/CMakeSettings.json new file mode 100644 index 00000000..21514b54 --- /dev/null +++ b/CMakeSettings.json @@ -0,0 +1,50 @@ +{ + "configurations": [ + { + "name": "x64-Debug", + "generator": "Ninja", + "configurationType": "Debug", + "inheritEnvironments": [ "msvc_x64_x64" ], + "buildRoot": "${projectDir}\\out\\build\\${name}", + "installRoot": "${projectDir}\\out\\install\\${name}", + "cmakeCommandArgs": "", + "buildCommandArgs": "", + "ctestCommandArgs": "", + "variables": [ + { + "name": "CMAKE_C_FLAGS", + "value": "/DWIN32 /D_WINDOWS", + "type": "STRING" + }, + { + "name": "CMAKE_CXX_FLAGS", + "value": "/DWIN32 /D_WINDOWS /GR /EHsc /bigobj", + "type": "STRING" + } + ] + }, + { + "name": "x64-Release", + "generator": "Ninja", + "configurationType": "Release", + "buildRoot": "${projectDir}\\out\\build\\${name}", + "installRoot": "${projectDir}\\out\\install\\${name}", + "cmakeCommandArgs": "", + "buildCommandArgs": "", + "ctestCommandArgs": "", + "inheritEnvironments": [ "msvc_x64_x64" ], + "variables": [ + { + "name": "CMAKE_C_FLAGS", + "value": "/DWIN32 /D_WINDOWS", + "type": "STRING" + }, + { + "name": "CMAKE_CXX_FLAGS", + "value": "/DWIN32 /D_WINDOWS /GR /EHsc /bigobj", + "type": "STRING" + } + ] + } + ] +} \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 0d3ef806..760ecac9 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -683,7 +683,7 @@ void save_results(bool info, double elapsed_seconds) void project_gui() { - if (ImGui::Begin("full_lidar_odometry_gui", &full_lidar_odometry_gui)) + if (ImGui::Begin("Settings", &full_lidar_odometry_gui)) { ImGui::Checkbox("simple_gui", &simple_gui); if (ImGui::IsItemHovered()) @@ -2112,7 +2112,7 @@ void display() ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background color", (float *)¶ms.clear_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float *)¶ms.clear_color, ImGuiColorEditFlags_NoInputs); bg_color = params.clear_color; diff --git a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp index 9dc43a7e..d47957ae 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -853,7 +853,7 @@ void display() ImGui::Separator(); ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); if (idToSn.size() == 2) { ImGui::ColorEdit3(idToSn.at(0).c_str(), (float*)&pc_color, ImGuiColorEditFlags_NoInputs); @@ -861,8 +861,8 @@ void display() } else { - ImGui::ColorEdit3("Cloud 1 color", (float*)&pc_color, ImGuiColorEditFlags_NoInputs); - ImGui::ColorEdit3("Cloud 2 color", (float*)&pc_color2, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Point cloud 1", (float*)&pc_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Point cloud 2", (float*)&pc_color2, ImGuiColorEditFlags_NoInputs); } ImGui::EndMenu(); 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 e33528b1..4ffa5b05 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -53,7 +53,6 @@ std::vector appShortcuts(80, { "", "", "" }); #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; -ImVec4 clear_color = ImVec4(0.8f, 0.8f, 0.8f, 1.00f); ImVec4 pc_color = ImVec4(1.0f, 0.0f, 0.0f, 1.00f); ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.00f); @@ -133,11 +132,7 @@ std::vector> rgd_nn; std::vector> mean_cov; bool show_mean_cov = false; bool show_rgd_nn = false; - -void optimize(); -std::vector> get_nn(); -void draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd); -std::vector> get_mean_cov(); +bool show_imu_data = false; namespace photos { @@ -149,6 +144,8 @@ namespace photos int photo_height_cam0 = 0; } +/////////////////////////////////////////////////////////////////////////////////// + static ImVec2 DisplayImageFit(ImTextureID tex, int tex_w, int tex_h, bool allow_upscale = true) { ImVec2 avail = ImGui::GetContentRegionAvail(); @@ -208,1385 +205,1433 @@ void render_nearest_photo(double ts) { glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, photo_width_cam0, photo_height_cam0, 0, GL_RGB, GL_UNSIGNED_BYTE, imgToShow.data); glBindTexture(GL_TEXTURE_2D, 0); - } -void project_gui() -{ - if (ImGui::Begin("CAM0")) { - if (photos::photo_texture_cam0) - { - using namespace photos; - ImGui::Text("fn name: %s ", photos::imgToShowFn.c_str()); - ImGui::Text("timestamp: %s", std::to_string(photos::nearestTs).c_str()); - // get available size - DisplayImageFit((ImTextureID)photos::photo_texture_cam0, photo_width_cam0, photo_height_cam0); - } - ImGui::End(); - } +void optimize() +{ - if (ImGui::Begin("main gui window")) - { - ImGui::ColorEdit3("clear color", (float *)&clear_color); - ImGui::ColorEdit3("pc_color_lidar_1", (float *)&pc_color); - ImGui::ColorEdit3("pc_color_lidar_2", (float *)&pc_color2); +#if 0 + rgd_nn.clear(); - ImGui::InputInt("point_size", &point_size); - if (point_size < 1) - { - point_size = 1; - } + // NDT::GridParameters rgd_params_sc; - if (is_init) - { - ImGui::InputInt("number_of_points_threshold", &number_of_points_threshold); - if (number_of_points_threshold < 0) - { - number_of_points_threshold = 0; - } - } + // rgd_params_sc.resolution_X = distance_bucket; + // rgd_params_sc.resolution_Y = polar_angle_deg; + // rgd_params_sc.resolution_Z = azimutal_angle_deg; - ImGui::InputDouble("ahrs_gain", &ahrs_gain); - // ImGui::Checkbox("is_slerp", &is_slerp); + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // std::vector points_local_sf; + std::vector points_local; + auto tr = all_data[index_rendered_points_local].poses; + auto trmm = all_data[index_rendered_points_local].poses; - ImGui::InputInt("index_rendered_points_local", &index_rendered_points_local); - if (index_rendered_points_local < 0) - { - index_rendered_points_local = 0; - } - if (index_rendered_points_local >= all_data.size()) + for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - index_rendered_points_local = all_data.size() - 1; - } + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); - if (is_init) - { - if (ImGui::Button("load data")) - { - 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); - - std::sort(std::begin(input_file_names), std::end(input_file_names)); - - 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")) - { - laz_files.push_back(fileName); - all_file_names.push_back(fileName); - } - else if (fileName.ends_with(".csv")) - csv_files.push_back(fileName); - else if (fileName.ends_with(".sn")) - sn_files.push_back(fileName); - else if (fileName.ends_with(".jpg")) { - 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; - } - } - } - }); + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; - if (input_file_names.size() > 0 && laz_files.size() == csv_files.size()) + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + // Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + // Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); + if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) { - working_directory = fs::path(input_file_names[0]).parent_path().string(); - - const auto calibrationFile = (fs::path(working_directory) / "calibration.json").string(); - const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); - imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); - std::cout << "imuSnToUse: " << imuSnToUse << std::endl; - if (!preloadedCalibration.empty()) - { - std::cout << "Loaded calibration for: \n"; - for (const auto &[sn, _] : preloadedCalibration) - std::cout << " -> " << sn << std::endl; - } - 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 << "Number of found photos: " << photos_files.size() << std::endl; - - - fs::path wdp = fs::path(input_file_names[0]).parent_path(); - wdp /= "preview"; - if (!fs::exists(wdp)) - fs::create_directory(wdp); - - working_directory_preview = wdp.string(); + // double polar_angle_deg_l = atan2(all_data[index_rendered_points_local].points_local[i].point.y(), all_data[index_rendered_points_local].points_local[i].point.x()) / M_PI * 180.0; + // double azimutal_angle_deg_l = acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; + all_data[index_rendered_points_local].points_local[i].index_pose = index_pose; + points_local.push_back(all_data[index_rendered_points_local].points_local[i]); - for (size_t i = 0; i < input_file_names.size(); i++) - std::cout << input_file_names[i] << std::endl; + /////////////////////////////////////////////////////// + // Point3Di p_sl = all_data[index_rendered_points_local].points_local[i]; + // p_sl.point.x() = r_l; + // p_sl.point.y() = polar_angle_deg_l; + // p_sl.point.z() = azimutal_angle_deg_l; - std::cout << "loading imu" << std::endl; - std::vector, FusionVector, FusionVector>> imu_data; + // points_local_sf.push_back(p_sl); + } + } + } - 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; - auto imu = load_imu(imufn.c_str(), imuNumberToUse); - std::cout << imufn << " with mapping " << snFn << std::endl; - imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); - } + //std::cout << "points_local.size(): " << points_local.size() << std::endl; + // points_local + // points_local_sf - // sort IMU data - // imu_data - std::sort(imu_data.begin(), imu_data.end(), - [](const std::tuple, FusionVector, FusionVector> &a, const std::tuple, FusionVector, FusionVector> &b) - { - return std::get<0>(a).first < std::get<0>(b).first; - }); - - std::cout - << "loading points" << std::endl; - std::vector> pointsPerFile; - pointsPerFile.resize(laz_files.size()); - // std::vector> indexesPerFile; - - std::mutex mtx; - std::cout << "start std::transform" << std::endl; - - std::transform(std::execution::par_unseq, std::begin(laz_files), std::end(laz_files), std::begin(pointsPerFile), [&](const std::string &fn) - { - // Load mapping from id to sn - fs::path fnSn(fn); - fnSn.replace_extension(".sn"); - - // GetId of Imu to use - const auto idToSn = MLvxCalib::GetIdToSnMapping(fnSn.string()); - auto calibration = MLvxCalib::CombineIntoCalibration(idToSn, preloadedCalibration); - auto data = load_point_cloud(fn.c_str(), true, filter_threshold_xy_inner, filter_threshold_xy_outer, calibration); - - if (fn == laz_files.front()) - { - fs::path calibrationValidtationFile = wdp / "calibrationValidation.asc"; - - std::ofstream testPointcloud{calibrationValidtationFile.c_str()}; - for (const auto &p : data) - testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid << "\n"; - } - - std::unique_lock lck(mtx); - for (const auto &[id, calib] : calibration) - { - std::cout << " id : " << id << std::endl; - std::cout << calib.matrix() << std::endl; - } - return data; - // std::cout << fn << std::endl; - // - }); - std::cout << "std::transform finished" << std::endl; - - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - ahrs.settings.convention = FusionConventionNwu; - - if (fusionConventionEnu) - ahrs.settings.convention = FusionConventionEnu; - - if (fusionConventionNed) - ahrs.settings.convention = FusionConventionNed; - - ahrs.settings.gain = ahrs_gain; - - std::map> trajectory; - - int counter = 1; - static bool first = true; - - static double last_ts; - - for (const auto &[timestamp_pair, gyr, acc] : imu_data) - { - const FusionVector gyroscope = {static_cast(gyr.axis.x * 180.0 / M_PI), static_cast(gyr.axis.y * 180.0 / M_PI), static_cast(gyr.axis.z * 180.0 / M_PI)}; - // const FusionVector gyroscope = {static_cast(gyr.axis.x), static_cast(gyr.axis.y), static_cast(gyr.axis.z)}; - const FusionVector accelerometer = {acc.axis.x, acc.axis.y, acc.axis.z}; + std::vector + point_cloud_global; + std::vector point_cloud_global_sc; - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - first = false; - // last_ts = timestamp_pair.first; - } - else - { - double curr_ts = timestamp_pair.first; - - double ts_diff = curr_ts - last_ts; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - - /*if (ts_diff < 0) - { - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - } - - if (ts_diff < 0.01) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - else - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - }*/ - } + for (int i = 0; i < points_local.size(); i++) + { + Point3Di pg = points_local[i]; + pg.point = tr[points_local[i].index_pose] * pg.point; + point_cloud_global.push_back(pg); + double r_g = pg.point.norm(); + point_cloud_global_sc.emplace_back(r_g, atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0, acos(pg.point.z() / r_g) / M_PI * 180.0); + } - last_ts = timestamp_pair.first; - // - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{quat.element.w, quat.element.x, quat.element.y, quat.element.z}; - Eigen::Affine3d t{Eigen::Matrix4d::Identity()}; - t.rotate(d); - - trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); - counter++; - if (counter % 100 == 0) - printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw, counter++, imu_data.size()); - - // log it for implot - imu_data_plot.timestampLidar.push_back(timestamp_pair.first); - imu_data_plot.angX.push_back(gyr.axis.x); - imu_data_plot.angY.push_back(gyr.axis.y); - imu_data_plot.angZ.push_back(gyr.axis.z); - imu_data_plot.accX.push_back(acc.axis.x); - imu_data_plot.accY.push_back(acc.axis.y); - imu_data_plot.accZ.push_back(acc.axis.z); - imu_data_plot.yaw.push_back(euler.angle.yaw); - imu_data_plot.pitch.push_back(euler.angle.pitch); - imu_data_plot.roll.push_back(euler.angle.roll); - } + NDT::GridParameters rgd_params_sc; - std::vector> timestamps; - std::vector poses; - for (const auto &t : trajectory) - { - timestamps.emplace_back(t.first, t.second.second); - Eigen::Affine3d m; - m.matrix() = t.second.first; - poses.push_back(m); - } + rgd_params_sc.resolution_X = distance_bucket; + rgd_params_sc.resolution_Y = polar_angle_deg; + rgd_params_sc.resolution_Z = azimutal_angle_deg; + Eigen::Vector3d b(rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z); - int number_of_points = 0; - for (const auto &pp : pointsPerFile) - number_of_points += pp.size(); + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params_sc, buckets, point_cloud_global, point_cloud_global_sc); + //update_rgd(rgd_params_sc, buckets, point_cloud_global, {0, 0, 0}); - std::cout << "number of points: " << number_of_points << std::endl; + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; - std::cout << "start indexing points" << std::endl; + for (int i = 0; i < point_cloud_global.size(); i++) + { - // std::vector points_global; - std::vector points_local; - std::vector lidar_ids; + auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); + //auto index_of_bucket = get_rgd_index(point_cloud_global[i].point, b); + auto bucket_it = buckets.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets.end()) + { + continue; + } + auto& this_bucket = bucket_it->second; - Eigen::Affine3d m_prev; - Eigen::Affine3d m_next; + rgd_nn.emplace_back(point_cloud_global[i].point, this_bucket.mean); - Eigen::Quaterniond q_prev; - Eigen::Quaterniond q_next; - Eigen::Quaterniond q; + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); + const double threshold = 100000.0; - double time_prev; - double time_next; - double curr_time; + if ((infm.array() > threshold).any()) + { + continue; + } + if ((infm.array() < -threshold).any()) + { + continue; + } - double t; + // std::cout << infm << std::endl; - for (int i = 0; i < pointsPerFile.size(); i++) - { - std::cout << "indexed: " << i + 1 << " of " << pointsPerFile.size() << " files" << std::endl; - for (const auto &pp : pointsPerFile[i]) - { - auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), pp.timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); - - int index_pose = std::distance(timestamps.begin(), lower) - 1; - - if (index_pose >= 0 && index_pose < poses.size()) - { - auto ppp = pp; - // Eigen::Affine3d m = poses[index_pose]; - /*if (is_slerp) - { - if (index_pose > 0) - { - m_prev = poses[index_pose - 1]; - m_next = poses[index_pose]; - - q_prev = Eigen::Quaterniond(m_prev.rotation()); - q_next = Eigen::Quaterniond(m_next.rotation()); - - time_prev = timestamps[index_pose - 1].first; - time_next = timestamps[index_pose].first; - curr_time = ppp.timestamp; - - t = (curr_time - time_prev) / (time_next - time_prev); - q = q_prev.slerp(t, q_next); - - m.linear() = q.toRotationMatrix(); - } - }*/ - - points_local.push_back(ppp); - - // ppp.point = m * ppp.point; - lidar_ids.push_back(pp.lidarid); - // points_global.push_back(ppp); - } - - if (points_local.size() > number_of_points_threshold) - { - // all_points_local.push_back(points_global); - indexes_to_filename.push_back(i); - // points_global.clear(); - // all_lidar_ids.push_back(lidar_ids); - - /////////////////////////////////////// - AllData data; - data.points_local = points_local; - data.lidar_ids = lidar_ids; - - for (int i = 0; i < timestamps.size(); i++) - { - if (timestamps[i].first >= points_local[0].timestamp && timestamps[i].first <= points_local[points_local.size() - 1].timestamp) - { - data.timestamps.push_back(timestamps[i]); - data.poses.push_back(poses[i]); - } - } - - // correct points timestamps - if (data.timestamps.size() > 2) - { - double ts_begin = data.timestamps[0].first; - double ts_step = (data.timestamps[data.timestamps.size() - 1].first - data.timestamps[0].first) / data.points_local.size(); - - //std::cout << "ts_begin " << ts_begin << std::endl; - //std::cout << "ts_step " << ts_step << std::endl; - //std::cout << "ts_end " << data.timestamps[data.timestamps.size() - 1].first << std::endl; - - for (int pp = 0; pp < data.points_local.size(); pp++) - data.points_local[pp].timestamp = ts_begin + pp * ts_step; - } - - all_data.push_back(data); - - points_local.clear(); - lidar_ids.clear(); - ////////////////////////////////////// - } - } - } + const Eigen::Affine3d& m_pose = tr[points_local[i].index_pose]; + //std::cout << "points_local[i].index_pose " << points_local[i].index_pose << std::endl; + const Eigen::Vector3d& p_s = points_local[i].point; + const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + double delta_x; + double delta_y; + double delta_z; - std::cout << "indexing points finished" << std::endl; + //// + Eigen::Matrix jacobian; + // TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - if (all_data.size() > 0) - { - is_init = false; - 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; + point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + p_s.x(), p_s.y(), p_s.z(), this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); - std::cout << "condition: input_file_names.size() > 0 && laz_files.size() == csv_files.size() && laz_files.size() == sn_files.size() NOT SATISFIED!!!" << std::endl; - } - } - } + // std::cout << "delta_x: " << delta_x << std::endl; + // std::cout << "delta_y: " << delta_y << std::endl; + // std::cout << "delta_z: " << delta_z << std::endl; - if (all_file_names.size() > 0) - { - if (index_rendered_points_local >= 0 && index_rendered_points_local < indexes_to_filename.size()) + if (sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z) < 0.001) { - std::string fn = all_file_names[indexes_to_filename[index_rendered_points_local]]; - ImGui::Text(fn.c_str()); - double ts = all_data[index_rendered_points_local].points_local[0].timestamp; // * 1e9; - ImGui::Text((std::string("ts: ") + std::to_string(ts)).c_str()); - render_nearest_photo(ts); + continue; } - } - if (ImGui::Button("save point cloud")) - { - std::string output_file_name = ""; - output_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter); - std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); - if (output_file_name.size() > 0) - { - std::vector pointcloud; - std::vector intensity; - std::vector timestamps; + int c = points_local[i].index_pose * 6; - /*if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + // std::mutex &m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; + // std::unique_lock lck(m); + int ir = tripletListB.size(); + + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) { - for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + if (jacobian(row, col) != 0.0) { - pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); - intensity.push_back(all_points_local[index_rendered_points_local][i].intensity); - timestamps.push_back(all_points_local[index_rendered_points_local][i].timestamp); + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); } - }*/ - - if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) - std::cout << "problem with saving file: " << output_file_name << std::endl; + } } - } - - ImGui::Text("---------------------------------------------"); - ImGui::InputDouble("distance_bucket", &distance_bucket); - ImGui::InputDouble("polar_angle_deg", &polar_angle_deg); - ImGui::InputDouble("azimutal_angle_deg", &azimutal_angle_deg); - ImGui::InputDouble("max_distance_lidar", &max_distance_lidar); - ImGui::InputInt("robust_and_accurate_lidar_odometry_iterations", &robust_and_accurate_lidar_odometry_iterations); - ImGui::Checkbox("useMultithread", &useMultithread); - ImGui::InputDouble("wx", &wx); - ImGui::InputDouble("wy", &wy); - ImGui::InputDouble("wz", &wz); - ImGui::InputDouble("wom", &wom); - ImGui::InputDouble("wfi", &wfi); - ImGui::InputDouble("wka", &wka); - ImGui::Text("---------------------------------------------"); + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); - if (ImGui::Button("optimize")) - optimize(); + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); - if (ImGui::Button("get_nn")) - rgd_nn = get_nn(); + /// + } - ImGui::Checkbox("show show_rgd_nn", &show_rgd_nn); + std::vector> odo_edges; + for (size_t i = 1; i < tr.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } - if (ImGui::Button("get_mean_cov")) - mean_cov = get_mean_cov(); + std::vector poses; + std::vector poses_desired; - ImGui::Checkbox("show_mean_cov", &show_mean_cov); + for (size_t i = 0; i < tr.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(tr[i])); + } + for (size_t i = 0; i < trmm.size(); i++) + { + poses_desired.push_back(pose_tait_bryan_from_affine_matrix(trmm[i])); + } - if (ImGui::Button("debug text")) + for (size_t i = 0; i < odo_edges.size(); i++) { - double max_diff = 0; - std::cout << std::setprecision(20); - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) - { - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka); + + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + relative_pose_measurement_odo(0, 0), + relative_pose_measurement_odo(1, 0), + relative_pose_measurement_odo(2, 0), + relative_pose_measurement_odo(3, 0), + relative_pose_measurement_odo(4, 0), + relative_pose_measurement_odo(5, 0)); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); + tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); + tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + + tripletListP.emplace_back(ir, ir, wx); + tripletListP.emplace_back(ir + 1, ir + 1, wy); + tripletListP.emplace_back(ir + 2, ir + 2, wz); + tripletListP.emplace_back(ir + 3, ir + 3, wom); + tripletListP.emplace_back(ir + 4, ir + 4, wfi); + tripletListP.emplace_back(ir + 5, ir + 5, wka); + } + + int ic = 0; + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, ic * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + + /*tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000);*/ + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), tr.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(tr.size() * 6, tr.size() * 6); + Eigen::SparseMatrix AtPB(tr.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + // AtPA += AtPAndt.sparseView(); + // AtPB += AtPBndt.sparseView(); + + // Eigen::SparseMatrix AtPA_I(tr.size() * 6, tr.size() * 6); + // AtPA_I.setIdentity(); + // AtPA += AtPA_I; + + Eigen::SimplicialCholesky> + solver(AtPA); + Eigen::SparseMatrix x = solver.solve(AtPB); + std::vector h_x; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + h_x.push_back(it.value()); + + std::cout << it.value() << " "; + } + } + + if (h_x.size() == 6 * tr.size()) + { + int counter = 0; + + for (size_t i = 0; i < tr.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(tr[i]); + auto prev_pose = pose; + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + // Eigen::Vector3d p1(prev_pose.px, prev_pose.py, prev_pose.pz); + // Eigen::Vector3d p2(pose.px, pose.py, pose.pz); + + // if ((p1 - p2).norm() < 1.0) + //{ + tr[i] = affine_matrix_from_pose_tait_bryan(pose); + //} + } + all_data[index_rendered_points_local].poses = tr; + } + } +#endif + + NDT::GridParameters rgd_params_sc; + + rgd_params_sc.resolution_X = distance_bucket; + rgd_params_sc.resolution_Y = polar_angle_deg; + rgd_params_sc.resolution_Z = azimutal_angle_deg; + + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + std::vector tr = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory; + std::vector trmm = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory_motion_model; + + std::vector points_local_sf; + std::vector points_local; + + for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + all_data[index_rendered_points_local].points_local[i].index_pose = index_pose; + // Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + // Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); + if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + double polar_angle_deg_l = atan2(all_data[index_rendered_points_local].points_local[i].point.y(), all_data[index_rendered_points_local].points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; - int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + points_local.push_back(all_data[index_rendered_points_local].points_local[i]); - if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) - { - std::cout << index_pose << " total nr of poses: [" << all_data[index_rendered_points_local].poses.size() << "] " << all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first << " ts point: " << all_data[index_rendered_points_local].points_local[i].timestamp << " pose ts: " << all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; + /////////////////////////////////////////////////////// + Point3Di p_sl = all_data[index_rendered_points_local].points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; - if (fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) - { - // std::cout << all_data[index_rendered_points_local].points_local[i].timestamp << std::endl; - max_diff = fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); - } - } + points_local_sf.push_back(p_sl); } - std::cout << "max_diff " << max_diff << std::endl; - std::cout << "----------------" << std::endl; - for (int k = 0; k < all_data[index_rendered_points_local].timestamps.size(); k++) - std::cout << all_data[index_rendered_points_local].timestamps[k].first << std::endl; } } - static bool show_imu_data = false; - ImGui::Checkbox("show_imu_data", &show_imu_data); + std::cout << "optimize_sf2" << std::endl; + for (int iter = 0; iter < robust_and_accurate_lidar_odometry_iterations; iter++) + { + optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, useMultithread, wx, wy, wz, wom, wfi, wka); + trmm = tr; + } - ImGui::End(); + all_data[index_rendered_points_local].poses = tr; + } + + return; +} + +std::vector> get_nn() +{ + std::vector> nn; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } + + auto worker_data = all_data[index_rendered_points_local]; + + // index data + for (int i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + { + worker_data.points_local[i].index_pose = index_pose; + } + else + { + worker_data.points_local[i].index_pose = -1; + } + } + + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + + rgd_params.resolution_X = distance_bucket; // distance bucket + rgd_params.resolution_Y = polar_angle_deg; // polar angle deg + rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (int i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) + { + double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } - if (show_imu_data) - { - ImGui::Begin("ImuData"); - if (imu_data_plot.timestampLidar.size() > 0) - { - static double x_min = imu_data_plot.timestampLidar.front(); - static double x_max = x_min + 20.0; - double annotation = 0; - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) - { - 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))) - { - ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); - ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); - ImPlot::PlotLine("accX", imu_data_plot.timestampLidar.data(), imu_data_plot.accX.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("accY", imu_data_plot.timestampLidar.data(), imu_data_plot.accY.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("accZ", imu_data_plot.timestampLidar.data(), imu_data_plot.accZ.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); - ImPlot::EndPlot(); - } + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc); - if (ImPlot::BeginPlot("Imu - gyro", ImVec2(-1, 0))) - { - ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); - ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); - ImPlot::PlotLine("angX", imu_data_plot.timestampLidar.data(), imu_data_plot.angX.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("angY", imu_data_plot.timestampLidar.data(), imu_data_plot.angY.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("angZ", imu_data_plot.timestampLidar.data(), imu_data_plot.angZ.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); - ImPlot::EndPlot(); - } + ///////////// + // std::vector> nn; + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); - 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); - ImPlot::PlotLine("yaw", imu_data_plot.timestampLidar.data(), imu_data_plot.yaw.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("pitch", imu_data_plot.timestampLidar.data(), imu_data_plot.pitch.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("roll", imu_data_plot.timestampLidar.data(), imu_data_plot.roll.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); - ImPlot::EndPlot(); - } + for (int i = 0; i < point_cloud_global_sc.size(); i++) + { + auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); - if (!photo_files_ts.empty() && ImPlot::BeginPlot("Photos")) - { - ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); - ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); - // plot photos timestamps - std::vector photo_timestamps; - std::vector dummy; - for (const auto &[ts, fn] : photo_files_ts) - { - photo_timestamps.push_back(static_cast(ts) / 1e9); - dummy.push_back(0.0); - } - ImPlot::PlotScatter("photos", photo_timestamps.data(), dummy.data(), (int)photo_timestamps.size()); - ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); - ImPlot::EndPlot(); - } - } + auto bucket_it = buckets.find(index_of_bucket); - if (ImGui::Button("Save 'ts gyr_x gyr_y gyr_z acc_x acc_y acc_z yaw_rad pitch_rad roll_rad' to csv")) + if (bucket_it != buckets.end()) { - std::string output_file_name = ""; - output_file_name = mandeye::fd::SaveFileDialog("Save imu data", {}, ""); - std::cout << "file to save: '" << output_file_name << "'" << std::endl; - - ofstream file; - file.open(output_file_name); - file << std::setprecision(20); - for (int i = 0; i < imu_data_plot.timestampLidar.size(); i++) - { - file << imu_data_plot.timestampLidar[i] << " " << - imu_data_plot.angX[i] << " " << - imu_data_plot.angY[i] << " " << - imu_data_plot.angZ[i] << " " << - imu_data_plot.accX[i] << " " << - imu_data_plot.accY[i] << " " << - imu_data_plot.accZ[i] << " " << - imu_data_plot.yaw[i] << " " << - imu_data_plot.pitch[i] << " " << - imu_data_plot.roll[i] << std::endl; - } + auto& this_bucket = bucket_it->second; + this_bucket.number_of_points++; + // const auto &curr_mean = points_global[i].point; + const auto& mean = this_bucket.mean; + + nn.emplace_back(point_cloud_global[i].point, mean); - file.close(); + ////////////// } - ImGui::End(); } } - return; + return nn; } -void display() +void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd = 3) { - ImGuiIO &io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - if (is_ortho) + 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 - glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100000, 100000); + 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; - glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100, 100); + glBegin(GL_LINE_LOOP); + glVertex3dv(tp0.data()); + glVertex3dv(tp1.data()); + glVertex3dv(tp2.data()); + glVertex3dv(tp3.data()); + glEnd(); + } + } +} - std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); +std::vector> get_mean_cov() +{ + std::vector> mc; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } - Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); - Eigen::Vector3d v(0, 1, 0); + auto worker_data = all_data[index_rendered_points_local]; - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * M_PI / 180.0; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + // index data + for (int i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); - Eigen::Vector3d v_t = m * v; + int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), - glm::vec3(v_t.x(), v_t.y(), v_t.z())); - std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); - } + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + worker_data.points_local[i].index_pose = index_pose; + else + worker_data.points_local[i].index_pose = -1; + } - glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg - if (!is_ortho) - { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + rgd_params.resolution_X = distance_bucket; // distance bucket + rgd_params.resolution_Y = polar_angle_deg; // polar angle deg + rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - viewTranslation.translate(rotation_center); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_x / 180.f, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_y / 180.f, Eigen::Vector3f::UnitZ())); + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (int i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) + { + double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - viewTranslation2.translate(-rotation_center); + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; - glLoadMatrixf(result.matrix().data()); - /* glTranslatef(translate_x, translate_y, translate_z); - glRotatef(rotate_x, 1.0, 0.0, 0.0); - glRotatef(rotate_y, 0.0, 0.0, 1.0);*/ - } - else - { - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - } + pp = pose * pp; - if (ImGui::GetIO().KeyCtrl) - { - glBegin(GL_LINES); - glColor3f(1.f, 1.f, 1.f); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); - glEnd(); - } + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; - if (show_axes) - { - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(100, 0.0f, 0.0f); + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 100, 0.0f); + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 100); - glEnd(); + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc); + + ///////////// + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + for (const auto& b : buckets) + { + auto& this_bucket = b.second; + + mc.emplace_back(this_bucket.mean, this_bucket.cov); + } } - // - /* glPointSize(point_size); - glBegin(GL_POINTS); - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) - { - for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) - { - if (all_lidar_ids[index_rendered_points_local][i] == 0) - { - glColor3f(pc_color.x, pc_color.y, pc_color.z); - } - else - { - glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); - } - glVertex3f(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); - } - } - glEnd(); - glPointSize(1);*/ + return mc; +} - glPointSize(point_size); - glBegin(GL_POINTS); - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) - { +void loadData() +{ + if (!is_init) + return; - double max_diff = 0.0; - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + 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); + + std::sort(std::begin(input_file_names), std::end(input_file_names)); + + std::for_each(std::begin(input_file_names), std::end(input_file_names), [&](const std::string& fileName) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); - int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; - if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + if (fileName.ends_with(".laz") || fileName.ends_with(".las")) { - if (fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + laz_files.push_back(fileName); + all_file_names.push_back(fileName); + } + else if (fileName.ends_with(".csv")) + csv_files.push_back(fileName); + else if (fileName.ends_with(".sn")) + sn_files.push_back(fileName); + else if (fileName.ends_with(".jpg")) { + 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()) { - // std::cout << index_pose << " " << all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; - max_diff = fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); + 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; + } } } - } - - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) - { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); - - int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + }); - // std::cout << index_pose << std::endl; + if (input_file_names.size() > 0 && laz_files.size() == csv_files.size()) + { + working_directory = fs::path(input_file_names[0]).parent_path().string(); - if (max_diff < 0.1) + const auto calibrationFile = (fs::path(working_directory) / "calibration.json").string(); + const auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); + imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); + std::cout << "imuSnToUse: " << imuSnToUse << std::endl; + if (!preloadedCalibration.empty()) + { + std::cout << "Loaded calibration for: \n"; + for (const auto& [sn, _] : preloadedCalibration) + std::cout << " -> " << sn << std::endl; + } + 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"; + /* { - if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) - { - // if (fabs(all_data[index_rendered_points_local].timestamps[index_pose].first - all_data[index_rendered_points_local].points_local[i].timestamp) < 0.001) - //{ - Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; - Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + "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 << "Number of found photos: " << photos_files.size() << std::endl; + + fs::path wdp = fs::path(input_file_names[0]).parent_path(); + wdp /= "preview"; + if (!fs::exists(wdp)) + fs::create_directory(wdp); - if (all_data[index_rendered_points_local].lidar_ids[i] == 0) - glColor3f(pc_color.x, pc_color.y, pc_color.z); - else - glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + working_directory_preview = wdp.string(); - glVertex3f(p.x(), p.y(), p.z()); - //} - } - } - } - } - glEnd(); - glPointSize(1); + for (size_t i = 0; i < input_file_names.size(); i++) + std::cout << input_file_names[i] << std::endl; - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) - { - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + std::cout << "loading imu" << std::endl; + std::vector, FusionVector, FusionVector>> imu_data; + + for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + 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; + auto imu = load_imu(imufn.c_str(), imuNumberToUse); + std::cout << imufn << " with mapping " << snFn << std::endl; + imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); + } - int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + // sort IMU data + // imu_data + std::sort(imu_data.begin(), imu_data.end(), + [](const std::tuple, FusionVector, FusionVector>& a, const std::tuple, FusionVector, FusionVector>& b) + { + return std::get<0>(a).first < std::get<0>(b).first; + }); - if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + std::cout + << "loading points" << std::endl; + std::vector> pointsPerFile; + pointsPerFile.resize(laz_files.size()); + // std::vector> indexesPerFile; + + std::mutex mtx; + std::cout << "start std::transform" << std::endl; + + std::transform(std::execution::par_unseq, std::begin(laz_files), std::end(laz_files), std::begin(pointsPerFile), [&](const std::string& fn) { - /*Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; - Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + // Load mapping from id to sn + fs::path fnSn(fn); + fnSn.replace_extension(".sn"); - if (all_data[index_rendered_points_local].lidar_ids[i] == 0) + // GetId of Imu to use + const auto idToSn = MLvxCalib::GetIdToSnMapping(fnSn.string()); + auto calibration = MLvxCalib::CombineIntoCalibration(idToSn, preloadedCalibration); + auto data = load_point_cloud(fn.c_str(), true, filter_threshold_xy_inner, filter_threshold_xy_outer, calibration); + + if (fn == laz_files.front()) { - glColor3f(pc_color.x, pc_color.y, pc_color.z); + fs::path calibrationValidtationFile = wdp / "calibrationValidation.asc"; + + std::ofstream testPointcloud{ calibrationValidtationFile.c_str() }; + for (const auto& p : data) + testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid << "\n"; } - else + + std::unique_lock lck(mtx); + for (const auto& [id, calib] : calibration) { - glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + std::cout << " id : " << id << std::endl; + std::cout << calib.matrix() << std::endl; } - glVertex3f(p.x(), p.y(), p.z());*/ + return data; + // std::cout << fn << std::endl; + // + }); + std::cout << "std::transform finished" << std::endl; - Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; - glLineWidth(2.0); - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + FusionAhrs ahrs; + FusionAhrsInitialise(&ahrs); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + if (fusionConventionNwu) + ahrs.settings.convention = FusionConventionNwu; - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); - glEnd(); - glLineWidth(1.0); - break; - } - } - } + if (fusionConventionEnu) + ahrs.settings.convention = FusionConventionEnu; - if (show_rgd_nn) - { - glColor3f(0, 0, 0); - glBegin(GL_LINES); - for (const auto &nn : rgd_nn) - { - glVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); - glVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); - } - glEnd(); - } + if (fusionConventionNed) + ahrs.settings.convention = FusionConventionNed; - if (show_mean_cov) - { - for (const auto &mc : mean_cov) - draw_ellipse(mc.second, mc.first, Eigen::Vector3f(1, 0, 0), 1); - } + ahrs.settings.gain = ahrs_gain; - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + std::map> trajectory; - project_gui(); + int counter = 1; + static bool first = true; - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + static double last_ts; - glutSwapBuffers(); - glutPostRedisplay(); -} + for (const auto& [timestamp_pair, gyr, acc] : imu_data) + { + const FusionVector gyroscope = { static_cast(gyr.axis.x * 180.0 / M_PI), static_cast(gyr.axis.y * 180.0 / M_PI), static_cast(gyr.axis.z * 180.0 / M_PI) }; + // const FusionVector gyroscope = {static_cast(gyr.axis.x), static_cast(gyr.axis.y), static_cast(gyr.axis.z)}; + const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; -void mouse(int glut_button, int state, int x, int y) -{ - ImGuiIO &io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - int button = -1; - if (glut_button == GLUT_LEFT_BUTTON) - button = 0; - if (glut_button == GLUT_RIGHT_BUTTON) - button = 1; - if (glut_button == GLUT_MIDDLE_BUTTON) - button = 2; - if (button != -1 && state == GLUT_DOWN) - io.MouseDown[button] = true; - if (button != -1 && state == GLUT_UP) - io.MouseDown[button] = false; + if (first) + { + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); + first = false; + // last_ts = timestamp_pair.first; + } + else + { + double curr_ts = timestamp_pair.first; - static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && - glutMajorVersion < 3) - { - wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); - } + double ts_diff = curr_ts - last_ts; - if (!io.WantCaptureMouse) - { - if (glut_button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) - { - } + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - if (state == GLUT_DOWN) - { - mouse_buttons |= 1 << glut_button; + /*if (ts_diff < 0) + { + std::cout << "WARNING!!!!" << std::endl; + std::cout << "WARNING!!!!" << std::endl; + std::cout << "WARNING!!!!" << std::endl; + std::cout << "WARNING!!!!" << std::endl; + std::cout << "WARNING!!!!" << std::endl; + std::cout << "WARNING!!!!" << std::endl; + } + + if (ts_diff < 0.01) + { + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); + } + else + { + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); + }*/ + } + + last_ts = timestamp_pair.first; + // + + FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); + + Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; + t.rotate(d); + + trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); + const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); + counter++; + if (counter % 100 == 0) + printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw, counter++, imu_data.size()); + + // log it for implot + imu_data_plot.timestampLidar.push_back(timestamp_pair.first); + imu_data_plot.angX.push_back(gyr.axis.x); + imu_data_plot.angY.push_back(gyr.axis.y); + imu_data_plot.angZ.push_back(gyr.axis.z); + imu_data_plot.accX.push_back(acc.axis.x); + imu_data_plot.accY.push_back(acc.axis.y); + imu_data_plot.accZ.push_back(acc.axis.z); + imu_data_plot.yaw.push_back(euler.angle.yaw); + imu_data_plot.pitch.push_back(euler.angle.pitch); + imu_data_plot.roll.push_back(euler.angle.roll); } - else if (state == GLUT_UP) + + std::vector> timestamps; + std::vector poses; + for (const auto& t : trajectory) { - mouse_buttons = 0; + timestamps.emplace_back(t.first, t.second.second); + Eigen::Affine3d m; + m.matrix() = t.second.first; + poses.push_back(m); } - mouse_old_x = x; - mouse_old_y = y; - } -} - -int main(int argc, char* argv[]) -{ - try - { - initGL(&argc, argv, winTitle, display, mouse); - glutMainLoop(); + int number_of_points = 0; + for (const auto& pp : pointsPerFile) + number_of_points += pp.size(); - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplGLUT_Shutdown(); - ImGui::DestroyContext(); - ImPlot::DestroyContext(); - } - catch (const std::bad_alloc& e) - { - std::cerr << "System is out of memory : " << e.what() << std::endl; - mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception& e) - { - std::cout << e.what(); - } - catch (...) - { - std::cerr << "Unknown fatal error occurred." << std::endl; - } + std::cout << "number of points: " << number_of_points << std::endl; - return 0; -} + std::cout << "start indexing points" << std::endl; -void optimize() -{ + // std::vector points_global; + std::vector points_local; + std::vector lidar_ids; -#if 0 - rgd_nn.clear(); + Eigen::Affine3d m_prev; + Eigen::Affine3d m_next; - // NDT::GridParameters rgd_params_sc; + Eigen::Quaterniond q_prev; + Eigen::Quaterniond q_next; + Eigen::Quaterniond q; - // rgd_params_sc.resolution_X = distance_bucket; - // rgd_params_sc.resolution_Y = polar_angle_deg; - // rgd_params_sc.resolution_Z = azimutal_angle_deg; + double time_prev; + double time_next; + double curr_time; - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) - { - // std::vector points_local_sf; - std::vector points_local; - auto tr = all_data[index_rendered_points_local].poses; - auto trmm = all_data[index_rendered_points_local].poses; + double t; - for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + for (int i = 0; i < pointsPerFile.size(); i++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + std::cout << "indexed: " << i + 1 << " of " << pointsPerFile.size() << " files" << std::endl; + for (const auto& pp : pointsPerFile[i]) + { + auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), pp.timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); - int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + int index_pose = std::distance(timestamps.begin(), lower) - 1; - if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) - { - // Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; - // Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; - double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); - if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) + if (index_pose >= 0 && index_pose < poses.size()) { - // double polar_angle_deg_l = atan2(all_data[index_rendered_points_local].points_local[i].point.y(), all_data[index_rendered_points_local].points_local[i].point.x()) / M_PI * 180.0; - // double azimutal_angle_deg_l = acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; - all_data[index_rendered_points_local].points_local[i].index_pose = index_pose; - points_local.push_back(all_data[index_rendered_points_local].points_local[i]); + auto ppp = pp; + // Eigen::Affine3d m = poses[index_pose]; + /*if (is_slerp) + { + if (index_pose > 0) + { + m_prev = poses[index_pose - 1]; + m_next = poses[index_pose]; - /////////////////////////////////////////////////////// - // Point3Di p_sl = all_data[index_rendered_points_local].points_local[i]; - // p_sl.point.x() = r_l; - // p_sl.point.y() = polar_angle_deg_l; - // p_sl.point.z() = azimutal_angle_deg_l; + q_prev = Eigen::Quaterniond(m_prev.rotation()); + q_next = Eigen::Quaterniond(m_next.rotation()); - // points_local_sf.push_back(p_sl); - } - } - } + time_prev = timestamps[index_pose - 1].first; + time_next = timestamps[index_pose].first; + curr_time = ppp.timestamp; - //std::cout << "points_local.size(): " << points_local.size() << std::endl; - // points_local - // points_local_sf + t = (curr_time - time_prev) / (time_next - time_prev); + q = q_prev.slerp(t, q_next); - std::vector - point_cloud_global; - std::vector point_cloud_global_sc; + m.linear() = q.toRotationMatrix(); + } + }*/ - for (int i = 0; i < points_local.size(); i++) - { - Point3Di pg = points_local[i]; - pg.point = tr[points_local[i].index_pose] * pg.point; - point_cloud_global.push_back(pg); - double r_g = pg.point.norm(); - point_cloud_global_sc.emplace_back(r_g, atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0, acos(pg.point.z() / r_g) / M_PI * 180.0); - } + points_local.push_back(ppp); - NDT::GridParameters rgd_params_sc; + // ppp.point = m * ppp.point; + lidar_ids.push_back(pp.lidarid); + // points_global.push_back(ppp); + } - rgd_params_sc.resolution_X = distance_bucket; - rgd_params_sc.resolution_Y = polar_angle_deg; - rgd_params_sc.resolution_Z = azimutal_angle_deg; - Eigen::Vector3d b(rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z); + if (points_local.size() > number_of_points_threshold) + { + // all_points_local.push_back(points_global); + indexes_to_filename.push_back(i); + // points_global.clear(); + // all_lidar_ids.push_back(lidar_ids); - NDTBucketMapType buckets; - update_rgd_spherical_coordinates(rgd_params_sc, buckets, point_cloud_global, point_cloud_global_sc); - //update_rgd(rgd_params_sc, buckets, point_cloud_global, {0, 0, 0}); + /////////////////////////////////////// + AllData data; + data.points_local = points_local; + data.lidar_ids = lidar_ids; - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; + for (int i = 0; i < timestamps.size(); i++) + { + if (timestamps[i].first >= points_local[0].timestamp && timestamps[i].first <= points_local[points_local.size() - 1].timestamp) + { + data.timestamps.push_back(timestamps[i]); + data.poses.push_back(poses[i]); + } + } - for (int i = 0; i < point_cloud_global.size(); i++) - { + // correct points timestamps + if (data.timestamps.size() > 2) + { + double ts_begin = data.timestamps[0].first; + double ts_step = (data.timestamps[data.timestamps.size() - 1].first - data.timestamps[0].first) / data.points_local.size(); - auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); - //auto index_of_bucket = get_rgd_index(point_cloud_global[i].point, b); - auto bucket_it = buckets.find(index_of_bucket); - // no bucket found - if (bucket_it == buckets.end()) - { - continue; + //std::cout << "ts_begin " << ts_begin << std::endl; + //std::cout << "ts_step " << ts_step << std::endl; + //std::cout << "ts_end " << data.timestamps[data.timestamps.size() - 1].first << std::endl; + + for (int pp = 0; pp < data.points_local.size(); pp++) + data.points_local[pp].timestamp = ts_begin + pp * ts_step; + } + + all_data.push_back(data); + + points_local.clear(); + lidar_ids.clear(); + ////////////////////////////////////// + } } - auto &this_bucket = bucket_it->second; + } - rgd_nn.emplace_back(point_cloud_global[i].point, this_bucket.mean); + std::cout << "indexing points finished" << std::endl; - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); - const double threshold = 100000.0; + if (all_data.size() > 0) + { + is_init = false; + 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; + } +} - if ((infm.array() > threshold).any()) +void imu_data_gui() +{ + ImGui::Begin("ImuData"); + { + if (imu_data_plot.timestampLidar.size() > 0) + { + static double x_min = imu_data_plot.timestampLidar.front(); + static double x_max = x_min + 20.0; + double annotation = 0; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - continue; + if (all_data[index_rendered_points_local].timestamps.size() > 0) + annotation = all_data[index_rendered_points_local].timestamps.front().first; } - if ((infm.array() < -threshold).any()) + if (ImPlot::BeginPlot("Imu - acceleration 'm/s^2", ImVec2(-1, 0))) { - continue; + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + ImPlot::PlotLine("accX", imu_data_plot.timestampLidar.data(), imu_data_plot.accX.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine("accY", imu_data_plot.timestampLidar.data(), imu_data_plot.accY.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine("accZ", imu_data_plot.timestampLidar.data(), imu_data_plot.accZ.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); } - // std::cout << infm << std::endl; - - const Eigen::Affine3d &m_pose = tr[points_local[i].index_pose]; - //std::cout << "points_local[i].index_pose " << points_local[i].index_pose << std::endl; - const Eigen::Vector3d &p_s = points_local[i].point; - const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - double delta_x; - double delta_y; - double delta_z; - - //// - Eigen::Matrix jacobian; - // TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); - - // std::cout << "delta_x: " << delta_x << std::endl; - // std::cout << "delta_y: " << delta_y << std::endl; - // std::cout << "delta_z: " << delta_z << std::endl; - - if (sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z) < 0.001) + if (ImPlot::BeginPlot("Imu - gyro", ImVec2(-1, 0))) { - continue; + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + ImPlot::PlotLine("angX", imu_data_plot.timestampLidar.data(), imu_data_plot.angX.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine("angY", imu_data_plot.timestampLidar.data(), imu_data_plot.angY.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine("angZ", imu_data_plot.timestampLidar.data(), imu_data_plot.angZ.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); } - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); - - int c = points_local[i].index_pose * 6; - - // std::mutex &m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; - // std::unique_lock lck(m); - int ir = tripletListB.size(); + 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); + ImPlot::PlotLine("yaw", imu_data_plot.timestampLidar.data(), imu_data_plot.yaw.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine("pitch", imu_data_plot.timestampLidar.data(), imu_data_plot.pitch.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine("roll", imu_data_plot.timestampLidar.data(), imu_data_plot.roll.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); + } - for (int row = 0; row < 3; row++) + if (!photo_files_ts.empty() && ImPlot::BeginPlot("Photos")) { - for (int col = 0; col < 6; col++) + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + // plot photos timestamps + std::vector photo_timestamps; + std::vector dummy; + for (const auto& [ts, fn] : photo_files_ts) { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } + photo_timestamps.push_back(static_cast(ts) / 1e9); + dummy.push_back(0.0); } + ImPlot::PlotScatter("photos", photo_timestamps.data(), dummy.data(), (int)photo_timestamps.size()); + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); } - tripletListB.emplace_back(ir, 0, delta_x); - tripletListB.emplace_back(ir + 1, 0, delta_y); - tripletListB.emplace_back(ir + 2, 0, delta_z); - - tripletListP.emplace_back(ir, ir, infm(0, 0)); - tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); - tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); - tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); - tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); - tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); - tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); - tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); - tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); - - /// } - std::vector> odo_edges; - for (size_t i = 1; i < tr.size(); i++) + if (ImGui::Button("Save 'ts gyr_x gyr_y gyr_z acc_x acc_y acc_z yaw_rad pitch_rad roll_rad' to csv")) { - odo_edges.emplace_back(i - 1, i); + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save imu data", {}, ""); + std::cout << "file to save: '" << output_file_name << "'" << std::endl; + + ofstream file; + file.open(output_file_name); + file << std::setprecision(20); + for (int i = 0; i < imu_data_plot.timestampLidar.size(); i++) + { + file << imu_data_plot.timestampLidar[i] << " " << + imu_data_plot.angX[i] << " " << + imu_data_plot.angY[i] << " " << + imu_data_plot.angZ[i] << " " << + imu_data_plot.accX[i] << " " << + imu_data_plot.accY[i] << " " << + imu_data_plot.accZ[i] << " " << + imu_data_plot.yaw[i] << " " << + imu_data_plot.pitch[i] << " " << + imu_data_plot.roll[i] << std::endl; + } + + file.close(); } + } - std::vector poses; - std::vector poses_desired; + ImGui::End(); +} - for (size_t i = 0; i < tr.size(); i++) +void project_gui() +{ + if (ImGui::Begin("Settings")) + { + ImGui::PushItemWidth(ImGuiNumberWidth); + if (is_init) { - poses.push_back(pose_tait_bryan_from_affine_matrix(tr[i])); + ImGui::InputInt("number_of_points_threshold", &number_of_points_threshold); + if (number_of_points_threshold < 0) + number_of_points_threshold = 0; } - for (size_t i = 0; i < trmm.size(); i++) + + ImGui::InputDouble("AHRS gain", &ahrs_gain); + // ImGui::Checkbox("is_slerp", &is_slerp); + + ImGui::PopItemWidth(); + + if (index_rendered_points_local < 0) + index_rendered_points_local = 0; + + if (index_rendered_points_local >= all_data.size()) + index_rendered_points_local = all_data.size() - 1; + + if (all_file_names.size() > 0) { - poses_desired.push_back(pose_tait_bryan_from_affine_matrix(trmm[i])); + if (index_rendered_points_local >= 0 && index_rendered_points_local < indexes_to_filename.size()) + { + double ts = all_data[index_rendered_points_local].points_local[0].timestamp; // * 1e9; + render_nearest_photo(ts); + } } - for (size_t i = 0; i < odo_edges.size(); i++) + if (ImGui::Button("Save point cloud")) { - Eigen::Matrix relative_pose_measurement_odo; - relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, - poses_desired[odo_edges[i].first].px, - poses_desired[odo_edges[i].first].py, - poses_desired[odo_edges[i].first].pz, - poses_desired[odo_edges[i].first].om, - poses_desired[odo_edges[i].first].fi, - poses_desired[odo_edges[i].first].ka, - poses_desired[odo_edges[i].second].px, - poses_desired[odo_edges[i].second].py, - poses_desired[odo_edges[i].second].pz, - poses_desired[odo_edges[i].second].om, - poses_desired[odo_edges[i].second].fi, - poses_desired[odo_edges[i].second].ka); + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter); + std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; - Eigen::Matrix delta; - relative_pose_obs_eq_tait_bryan_wc_case1( - delta, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - relative_pose_measurement_odo(0, 0), - relative_pose_measurement_odo(1, 0), - relative_pose_measurement_odo(2, 0), - relative_pose_measurement_odo(3, 0), - relative_pose_measurement_odo(4, 0), - relative_pose_measurement_odo(5, 0)); + if (output_file_name.size() > 0) + { + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; - Eigen::Matrix jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka); + /*if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + { + for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + { + pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + intensity.push_back(all_points_local[index_rendered_points_local][i].intensity); + timestamps.push_back(all_points_local[index_rendered_points_local][i].timestamp); + } + }*/ - int ir = tripletListB.size(); + if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) + std::cout << "problem with saving file: " << output_file_name << std::endl; + } + } - int ic_1 = odo_edges[i].first * 6; - int ic_2 = odo_edges[i].second * 6; + ImGui::Separator(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("distance_bucket", &distance_bucket); + ImGui::InputDouble("polar_angle_deg", &polar_angle_deg); + ImGui::InputDouble("azimutal_angle_deg", &azimutal_angle_deg); + ImGui::InputDouble("max_distance_lidar", &max_distance_lidar); + ImGui::InputInt("robust_and_accurate_lidar_odometry_iterations", &robust_and_accurate_lidar_odometry_iterations); + ImGui::Checkbox("useMultithread", &useMultithread); + ImGui::InputDouble("wx", &wx); + ImGui::InputDouble("wy", &wy); + ImGui::InputDouble("wz", &wz); + ImGui::InputDouble("wom", &wom); + ImGui::InputDouble("wfi", &wfi); + ImGui::InputDouble("wka", &wka); + ImGui::PopItemWidth(); + ImGui::Separator(); - for (size_t row = 0; row < 6; row++) + if (ImGui::Button("Optimize")) + optimize(); + + ImGui::SameLine(); + + if (ImGui::Button("Print to console debug text")) + { + double max_diff = 0; + std::cout << std::setprecision(20); + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); - tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); - tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); - tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); - tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); - tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); - tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); - tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); - tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); - tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); - tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); - tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); - } + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; - tripletListB.emplace_back(ir, 0, delta(0, 0)); - tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); - tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); - tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); - tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); - tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + std::cout << index_pose << " total nr of poses: [" << all_data[index_rendered_points_local].poses.size() << "] " << all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first << " ts point: " << all_data[index_rendered_points_local].points_local[i].timestamp << " pose ts: " << all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; - tripletListP.emplace_back(ir, ir, wx); - tripletListP.emplace_back(ir + 1, ir + 1, wy); - tripletListP.emplace_back(ir + 2, ir + 2, wz); - tripletListP.emplace_back(ir + 3, ir + 3, wom); - tripletListP.emplace_back(ir + 4, ir + 4, wfi); - tripletListP.emplace_back(ir + 5, ir + 5, wka); + if (fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + { + // std::cout << all_data[index_rendered_points_local].points_local[i].timestamp << std::endl; + max_diff = fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); + } + } + } + std::cout << "max_diff " << max_diff << std::endl; + std::cout << "----------------" << std::endl; + for (int k = 0; k < all_data[index_rendered_points_local].timestamps.size(); k++) + std::cout << all_data[index_rendered_points_local].timestamps[k].first << std::endl; + } } + } - int ic = 0; - int ir = tripletListB.size(); - tripletListA.emplace_back(ir, ic * 6 + 0, 1); - tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); - tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); - tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); - tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); - tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + ImGui::End(); +} - /*tripletListP.emplace_back(ir, ir, 1000000); - tripletListP.emplace_back(ir + 1, ir + 1, 1000000); - tripletListP.emplace_back(ir + 2, ir + 2, 1000000); - tripletListP.emplace_back(ir + 3, ir + 3, 1000000); - tripletListP.emplace_back(ir + 4, ir + 4, 1000000); - tripletListP.emplace_back(ir + 5, ir + 5, 1000000);*/ - tripletListP.emplace_back(ir, ir, 1); - tripletListP.emplace_back(ir + 1, ir + 1, 1); - tripletListP.emplace_back(ir + 2, ir + 2, 1); - tripletListP.emplace_back(ir + 3, ir + 3, 1); - tripletListP.emplace_back(ir + 4, ir + 4, 1); - tripletListP.emplace_back(ir + 5, ir + 5, 1); +void display() +{ + ImGuiIO& io = ImGui::GetIO(); + glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - tripletListB.emplace_back(ir, 0, 0); - tripletListB.emplace_back(ir + 1, 0, 0); - tripletListB.emplace_back(ir + 2, 0, 0); - tripletListB.emplace_back(ir + 3, 0, 0); - tripletListB.emplace_back(ir + 4, 0, 0); - tripletListB.emplace_back(ir + 5, 0, 0); + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); - Eigen::SparseMatrix matA(tripletListB.size(), tr.size() * 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - Eigen::SparseMatrix AtPA(tr.size() * 6, tr.size() * 6); - Eigen::SparseMatrix AtPB(tr.size() * 6, 1); + viewLocal.translate(rotation_center); - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = (AtP)*matA; - AtPB = (AtP)*matB; - } + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + else + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); + viewLocal.translate(-rotation_center); - // AtPA += AtPAndt.sparseView(); - // AtPB += AtPBndt.sparseView(); + glLoadMatrixf(viewLocal.matrix().data()); + } + else + { + glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, -100000, 100000); - // Eigen::SparseMatrix AtPA_I(tr.size() * 6, tr.size() * 6); - // AtPA_I.setIdentity(); - // AtPA += AtPA_I; + glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, -100, 100); - Eigen::SimplicialCholesky> - solver(AtPA); - Eigen::SparseMatrix x = solver.solve(AtPB); - std::vector h_x; - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - h_x.push_back(it.value()); + std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); - std::cout << it.value() << " "; - } - } + Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v(0, 1, 0); - if (h_x.size() == 6 * tr.size()) - { - int counter = 0; + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * DEG_TO_RAD; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - for (size_t i = 0; i < tr.size(); i++) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(tr[i]); - auto prev_pose = pose; - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; + Eigen::Vector3d v_t = m * v; - // Eigen::Vector3d p1(prev_pose.px, prev_pose.py, prev_pose.pz); - // Eigen::Vector3d p2(pose.px, pose.py, pose.pz); + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), + v_center_t.x(), v_center_t.y(), v_center_t.z(), + v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), + glm::vec3(v_t.x(), v_t.y(), v_t.z())); + std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); - // if ((p1 - p2).norm() < 1.0) - //{ - tr[i] = affine_matrix_from_pose_tait_bryan(pose); - //} - } - all_data[index_rendered_points_local].poses = tr; - } + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); } -#endif - - NDT::GridParameters rgd_params_sc; - rgd_params_sc.resolution_X = distance_bucket; - rgd_params_sc.resolution_Y = polar_angle_deg; - rgd_params_sc.resolution_Z = azimutal_angle_deg; + // + /* glPointSize(point_size); + glBegin(GL_POINTS); + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + { + for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + { + if (all_lidar_ids[index_rendered_points_local][i] == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + glVertex3f(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + } + } + glEnd(); + glPointSize(1);*/ + glPointSize(point_size); + glBegin(GL_POINTS); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - std::vector tr = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory; - std::vector trmm = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory_motion_model; - std::vector points_local_sf; - std::vector points_local; + double max_diff = 0.0; + for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + if (fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + { + // std::cout << index_pose << " " << all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; + max_diff = fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); + } + } + } for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { @@ -1596,300 +1641,370 @@ void optimize() int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; - if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + // std::cout << index_pose << std::endl; + + if (max_diff < 0.1) { - all_data[index_rendered_points_local].points_local[i].index_pose = index_pose; - // Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; - // Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; - double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); - if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) { - double polar_angle_deg_l = atan2(all_data[index_rendered_points_local].points_local[i].point.y(), all_data[index_rendered_points_local].points_local[i].point.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; - - points_local.push_back(all_data[index_rendered_points_local].points_local[i]); + // if (fabs(all_data[index_rendered_points_local].timestamps[index_pose].first - all_data[index_rendered_points_local].points_local[i].timestamp) < 0.001) + //{ + Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; - /////////////////////////////////////////////////////// - Point3Di p_sl = all_data[index_rendered_points_local].points_local[i]; - p_sl.point.x() = r_l; - p_sl.point.y() = polar_angle_deg_l; - p_sl.point.z() = azimutal_angle_deg_l; + if (all_data[index_rendered_points_local].lidar_ids[i] == 0) + glColor3f(pc_color.x, pc_color.y, pc_color.z); + else + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); - points_local_sf.push_back(p_sl); + glVertex3f(p.x(), p.y(), p.z()); + //} } } } - - std::cout << "optimize_sf2" << std::endl; - for (int iter = 0; iter < robust_and_accurate_lidar_odometry_iterations; iter++) - { - optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, useMultithread, wx, wy, wz, wom, wfi, wka); - trmm = tr; - } - - all_data[index_rendered_points_local].poses = tr; } + glEnd(); + glPointSize(1); - return; -} - -std::vector> get_nn() -{ - std::vector> nn; if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ - // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; - // } - - auto worker_data = all_data[index_rendered_points_local]; - - // index data - for (int i = 0; i < worker_data.points_local.size(); i++) + for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool { return lhs.first < rhs; }); - int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; - if (index_pose >= 0 && index_pose < worker_data.poses.size()) - { - worker_data.points_local[i].index_pose = index_pose; - } - else + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) { - worker_data.points_local[i].index_pose = -1; - } - } + /*Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; - NDT::GridParameters rgd_params; - // rgd_params.resolution_X = 0.3; // distance bucket - // rgd_params.resolution_Y = 0.3; // polar angle deg - // rgd_params.resolution_Z = 0.3; // azimutal angle deg + if (all_data[index_rendered_points_local].lidar_ids[i] == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + glVertex3f(p.x(), p.y(), p.z());*/ - rgd_params.resolution_X = distance_bucket; // distance bucket - rgd_params.resolution_Y = polar_angle_deg; // polar angle deg - rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg + Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + glLineWidth(2.0); + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); - std::vector point_cloud_global; - std::vector points_local; + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); - std::vector point_cloud_global_sc; - std::vector points_local_sc; + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + glLineWidth(1.0); + break; + } + } + } - for (int i = 0; i < worker_data.points_local.size(); i++) + if (show_rgd_nn) + { + glColor3f(0, 0, 0); + glBegin(GL_LINES); + for (const auto &nn : rgd_nn) { - double r_l = worker_data.points_local[i].point.norm(); - if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) - { - double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + glVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); + glVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); + } + glEnd(); + } + + if (show_mean_cov) + { + for (const auto &mc : mean_cov) + draw_ellipse(mc.second, mc.first, Eigen::Vector3f(1, 0, 0), 1); + } - Eigen::Vector3d pp = worker_data.points_local[i].point; - // pps.x() = r; - // pps.y() = polar_angle_deg; - // pps.z() = azimutal_angle_deg; - // point_cloud_spherical_coordinates.push_back(pps); + showAxes(); - Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); - pp = pose * pp; + ShowMainDockSpace(); - Point3Di pg = worker_data.points_local[i]; - pg.point = pp; + view_kbd_shortcuts(); - point_cloud_global.push_back(pg); - points_local.push_back(worker_data.points_local[i]); + if (all_data.size() > 0) + { + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) + || ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true) + ) + index_rendered_points_local += 1; + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + || ImGui::IsKeyPressed(ImGuiKey_PageDown, true) + || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true) + ) + index_rendered_points_local -= 1; - /////////////////////////////////////////////////////// - Point3Di p_sl = worker_data.points_local[i]; - p_sl.point.x() = r_l; - p_sl.point.y() = polar_angle_deg_l; - p_sl.point.z() = azimutal_angle_deg_l; + if (index_rendered_points_local < 0) + index_rendered_points_local = 0; + if (index_rendered_points_local >= all_data.size()) + index_rendered_points_local = all_data.size() - 1; + } - points_local_sc.push_back(p_sl); - // - double r_g = pg.point.norm(); - double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; - double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + if (ImGui::BeginMainMenuBar()) + { + if (ImGui::Button("Load data")) + loadData(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select session to open for analyze (Ctrl+O)"); - Eigen::Vector3d p_sg = worker_data.points_local[i].point; - p_sg.x() = r_g; - p_sg.y() = polar_angle_deg_g; - p_sg.z() = azimutal_angle_deg_g; + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); - point_cloud_global_sc.push_back(p_sg); + if (ImGui::BeginMenu("View")) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Points size", &point_size); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("keyboard 1-9 keys"); + if (point_size < 1) + point_size = 1; + else if (point_size > 10) + point_size = 10; + + ImGui::Separator(); + + ImGui::MenuItem("Orthographic", "key O", &is_ortho); + if (is_ortho) + { + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; } - } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); - NDTBucketMapType buckets; - update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc); + ImGui::MenuItem("Show axes", "key X", &show_axes); + ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); - ///////////// - // std::vector> nn; - Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); - for (int i = 0; i < point_cloud_global_sc.size(); i++) - { - auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); + ImGui::Separator(); - auto bucket_it = buckets.find(index_of_bucket); - if (bucket_it != buckets.end()) + if (ImGui::MenuItem("Show rgd_nn", nullptr, &show_rgd_nn)) { - auto &this_bucket = bucket_it->second; - this_bucket.number_of_points++; - // const auto &curr_mean = points_global[i].point; - const auto &mean = this_bucket.mean; - - nn.emplace_back(point_cloud_global[i].point, mean); - - ////////////// + if (show_rgd_nn) + rgd_nn = get_nn(); } - } - } - return nn; -} -void draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd = 3) -{ + if (ImGui::MenuItem("Show mean covs", nullptr, &show_mean_cov)) + { + if (show_mean_cov) + mean_cov = get_mean_cov(); + } - Eigen::LLT> cholSolver(covar); - Eigen::Matrix3d transform = cholSolver.matrixL(); + ImGui::MenuItem("Show IMU data", nullptr, &show_imu_data); - 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()); + ImGui::Separator(); - 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 + if (ImGui::BeginMenu("Colors")) + { + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Point cloud 1", (float*)&pc_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Point cloud 2", (float*)&pc_color2, ImGuiColorEditFlags_NoInputs); - 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; + ImGui::EndMenu(); + } - glBegin(GL_LINE_LOOP); - glVertex3dv(tp0.data()); - glVertex3dv(tp1.data()); - glVertex3dv(tp2.data()); - glVertex3dv(tp3.data()); - glEnd(); + ImGui::EndMenu(); } - } -} + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Scene view relevant parameters"); -std::vector> get_mean_cov() -{ - std::vector> mc; - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) - { - // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ - // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; - // } + camMenu(); - auto worker_data = all_data[index_rendered_points_local]; + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); - // index data - for (int i = 0; i < worker_data.points_local.size(); i++) + if (all_data.size() > 0) { - auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + int tempIndex = index_rendered_points_local; + ImGui::Text("Index: "); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Index of rendered points local"); + ImGui::SameLine(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::SliderInt("##irpls", &tempIndex, 0, static_cast(all_data.size() - 1)); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + std::string fn = all_file_names[indexes_to_filename[index_rendered_points_local]]; + ImGui::Text(fn.c_str()); + ImGui::EndTooltip(); + } + ImGui::SameLine(); + ImGui::InputInt("##irpli", &tempIndex, 1, 10); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + std::string fn = all_file_names[indexes_to_filename[index_rendered_points_local]]; + ImGui::Text(fn.c_str()); + ImGui::EndTooltip(); + } + ImGui::PopItemWidth(); - int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; + if ((tempIndex >= 0) && (tempIndex < all_data.size())) + index_rendered_points_local = tempIndex; - if (index_pose >= 0 && index_pose < worker_data.poses.size()) - worker_data.points_local[i].index_pose = index_pose; - else - worker_data.points_local[i].index_pose = -1; - } + ImGui::SameLine(); + double ts = all_data[index_rendered_points_local].points_local[0].timestamp; // * 1e9; + ImGui::Text("Timestamp: %.6f [ns]", ts); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Click to copy to clipboard"); + if (ImGui::IsItemClicked()) + { + char tmp[64]; + snprintf(tmp, sizeof(tmp), "%.6f", ts); + ImGui::SetClipboardText(tmp); + } - NDT::GridParameters rgd_params; - // rgd_params.resolution_X = 0.3; // distance bucket - // rgd_params.resolution_Y = 0.3; // polar angle deg - // rgd_params.resolution_Z = 0.3; // azimutal angle deg + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + } - rgd_params.resolution_X = distance_bucket; // distance bucket - rgd_params.resolution_Y = polar_angle_deg; // polar angle deg - rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg + ImGui::SameLine(); + ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); - std::vector point_cloud_global; - std::vector points_local; + ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); - std::vector point_cloud_global_sc; - std::vector points_local_sc; + ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); + ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0, 0, 0, 0)); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); + ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); + if (ImGui::SmallButton("Info")) + info_gui = !info_gui; - for (int i = 0; i < worker_data.points_local.size(); i++) - { - double r_l = worker_data.points_local[i].point.norm(); - if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) - { - double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + ImGui::PopStyleVar(2); + ImGui::PopStyleColor(3); - Eigen::Vector3d pp = worker_data.points_local[i].point; - // pps.x() = r; - // pps.y() = polar_angle_deg; - // pps.z() = azimutal_angle_deg; - // point_cloud_spherical_coordinates.push_back(pps); + ImGui::EndMainMenuBar(); + } - Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + info_window(infoLines, appShortcuts); - pp = pose * pp; + if (compass_ruler) + drawMiniCompassWithRuler(); - Point3Di pg = worker_data.points_local[i]; - pg.point = pp; + if (photos::photo_texture_cam0) + { + if (ImGui::Begin("CAM0")) { + using namespace photos; + ImGui::Text("fn name: %s ", photos::imgToShowFn.c_str()); + ImGui::Text("timestamp: %s", std::to_string(photos::nearestTs).c_str()); + // get available size + DisplayImageFit((ImTextureID)photos::photo_texture_cam0, photo_width_cam0, photo_height_cam0); + } - point_cloud_global.push_back(pg); - points_local.push_back(worker_data.points_local[i]); + ImGui::End(); + } - /////////////////////////////////////////////////////// - Point3Di p_sl = worker_data.points_local[i]; - p_sl.point.x() = r_l; - p_sl.point.y() = polar_angle_deg_l; - p_sl.point.z() = azimutal_angle_deg_l; + if (show_imu_data) + imu_data_gui(); - points_local_sc.push_back(p_sl); - // - double r_g = pg.point.norm(); - double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; - double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + project_gui(); - Eigen::Vector3d p_sg = worker_data.points_local[i].point; - p_sg.x() = r_g; - p_sg.y() = polar_angle_deg_g; - p_sg.z() = azimutal_angle_deg_g; + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - point_cloud_global_sc.push_back(p_sg); - } - } + glutSwapBuffers(); + glutPostRedisplay(); +} - NDTBucketMapType buckets; - update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc); +void mouse(int glut_button, int state, int x, int y) +{ + ImGuiIO &io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + int button = -1; + if (glut_button == GLUT_LEFT_BUTTON) + button = 0; + if (glut_button == GLUT_RIGHT_BUTTON) + button = 1; + if (glut_button == GLUT_MIDDLE_BUTTON) + button = 2; + if (button != -1 && state == GLUT_DOWN) + io.MouseDown[button] = true; + if (button != -1 && state == GLUT_UP) + io.MouseDown[button] = false; - ///////////// - Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && + glutMajorVersion < 3) + { + wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + } - for (const auto &b : buckets) + if (!io.WantCaptureMouse) + { + if (glut_button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) { - auto &this_bucket = b.second; + } - mc.emplace_back(this_bucket.mean, this_bucket.cov); + if (state == GLUT_DOWN) + { + mouse_buttons |= 1 << glut_button; + } + else if (state == GLUT_UP) + { + mouse_buttons = 0; } + mouse_old_x = x; + mouse_old_y = y; } +} - return mc; +int main(int argc, char* argv[]) +{ + try + { + initGL(&argc, argv, winTitle, display, mouse); + + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + ImGui::DestroyContext(); + ImPlot::DestroyContext(); + } + catch (const std::bad_alloc& e) + { + std::cerr << "System is out of memory : " << e.what() << std::endl; + mandeye::fd::OutOfMemMessage(); + } + catch (const std::exception& e) + { + std::cout << e.what(); + } + catch (...) + { + std::cerr << "Unknown fatal error occurred." << std::endl; + } + + return 0; } \ No newline at end of file diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp index 1728674b..2bcc6752 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -872,26 +872,26 @@ void index_gui() ImGui::Separator(); ImGui::Text("Timestamps:"); - double t = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.front(); + double ts = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.front(); - ImGui::Text("First point: %.0f [ns]", t); + ImGui::Text("First point: %.0f [ns]", ts); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Click to copy to clipboard"); if (ImGui::IsItemClicked()) { char tmp[64]; - snprintf(tmp, sizeof(tmp), "%.0f", t); + snprintf(tmp, sizeof(tmp), "%.0f", ts); ImGui::SetClipboardText(tmp); } - t = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.back(); - ImGui::Text("Last point : %.0f [ns]", t); + ts = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.back(); + ImGui::Text("Last point : %.0f [ns]", ts); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Click to copy to clipboard"); if (ImGui::IsItemClicked()) { char tmp[64]; - snprintf(tmp, sizeof(tmp), "%.0f", t); + snprintf(tmp, sizeof(tmp), "%.0f", ts); ImGui::SetClipboardText(tmp); } } @@ -1188,14 +1188,12 @@ void display() for (auto& point_cloud : session.point_clouds_container.point_clouds) point_cloud.point_size = point_size; + //ImGui::MenuItem("show_imu_to_lio_diff", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + ImGui::Separator(); } ImGui::EndDisabled(); - //ImGui::MenuItem("show_imu_to_lio_diff", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); - - ImGui::Separator(); - ImGui::MenuItem("Orthographic", "key O", &is_ortho); if (is_ortho) { @@ -1243,19 +1241,19 @@ void display() if (ImGui::BeginMenu("Colors")) { - ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); ImGui::BeginDisabled(!(session.point_clouds_container.point_clouds.size() > 0)); { ImGui::BeginDisabled(!show_neighbouring_scans); { - ImGui::ColorEdit3("Neigbouring color", (float*)&pc_neigbouring_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Neighbours", (float*)&pc_neigbouring_color, ImGuiColorEditFlags_NoInputs); } ImGui::EndDisabled(); ImGui::Separator(); - ImGui::Text("Point cloud color:"); + ImGui::Text("Point cloud:"); float color[3]; if (session.point_clouds_container.point_clouds.size() > 0) diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index e237ed0e..4c2a5951 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -1494,72 +1494,6 @@ void loop_closure_gui() } } -bool load_project_settings(const std::string &file_name, ProjectSettings &_project_settings) -{ - std::cout << "loading file: '" << file_name << "'" << std::endl; - std::vector session_file_names; - - try - { - std::ifstream fs(file_name); - if (!fs.good()) - return false; - nlohmann::json data = nlohmann::json::parse(fs); - fs.close(); - - for (const auto &fn_json : data["session_file_names"]) - { - std::string fn = fn_json["session_file_name"]; - session_file_names.push_back(fn); - } - - std::cout << "------session file names-----" << std::endl; - for (const auto &fn : session_file_names) - { - std::cout << "'" << fn << "'" << std::endl; - } - - _project_settings.session_file_names = session_file_names; - - edges.clear(); - for (const auto &edge_json : data["loop_closure_edges"]) - { - Edge edge; - edge.index_from = edge_json["index_from"]; - edge.index_to = edge_json["index_to"]; - edge.is_fixed_fi = edge_json["is_fixed_fi"]; - edge.is_fixed_ka = edge_json["is_fixed_ka"]; - edge.is_fixed_om = edge_json["is_fixed_om"]; - edge.is_fixed_px = edge_json["is_fixed_px"]; - edge.is_fixed_py = edge_json["is_fixed_py"]; - edge.is_fixed_pz = edge_json["is_fixed_pz"]; - edge.relative_pose_tb.fi = edge_json["fi"]; - edge.relative_pose_tb.ka = edge_json["ka"]; - edge.relative_pose_tb.om = edge_json["om"]; - edge.relative_pose_tb.px = edge_json["px"]; - edge.relative_pose_tb.py = edge_json["py"]; - edge.relative_pose_tb.pz = edge_json["pz"]; - edge.relative_pose_tb_weights.fi = edge_json["w_fi"]; - edge.relative_pose_tb_weights.ka = edge_json["w_ka"]; - edge.relative_pose_tb_weights.om = edge_json["w_om"]; - edge.relative_pose_tb_weights.px = edge_json["w_px"]; - edge.relative_pose_tb_weights.py = edge_json["w_py"]; - edge.relative_pose_tb_weights.pz = edge_json["w_pz"]; - edge.index_session_from = edge_json["index_session_from"]; - edge.index_session_to = edge_json["index_session_to"]; - edges.push_back(edge); - } - return true; - } - catch (std::exception &e) - { - std::cout << "cant load project settings: " << e.what() << std::endl; - return false; - } - - return true; -} - void save_trajectories_to_laz(const Session &session, std::string output_file_name, float curve_consecutive_distance_meters, float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling) { std::vector pointcloud; @@ -2582,11 +2516,78 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking &observ return pos; } +bool load_project_settings(const std::string& file_name, ProjectSettings& _project_settings) +{ + std::cout << "Opening project file: '" << file_name << "'\n"; + + try + { + std::ifstream fs(file_name); + if (!fs.good()) + return false; + nlohmann::json data = nlohmann::json::parse(fs); + fs.close(); + + _project_settings.session_file_names.clear(); + + std::cout << "Contained sessions:\n"; + + for (const auto& fn_json : data["session_file_names"]) + { + std::string fn = fn_json["session_file_name"]; + _project_settings.session_file_names.push_back(fn); + std::cout << "'" << fn << "'"; + if (!fs::exists(fn)) + std::cout << " (WARNING: session file does not exist! Please manually adapt path)"; + std::cout << "\n"; + } + + edges.clear(); + for (const auto& edge_json : data["loop_closure_edges"]) + { + Edge edge; + edge.index_from = edge_json["index_from"]; + edge.index_to = edge_json["index_to"]; + edge.is_fixed_fi = edge_json["is_fixed_fi"]; + edge.is_fixed_ka = edge_json["is_fixed_ka"]; + edge.is_fixed_om = edge_json["is_fixed_om"]; + edge.is_fixed_px = edge_json["is_fixed_px"]; + edge.is_fixed_py = edge_json["is_fixed_py"]; + edge.is_fixed_pz = edge_json["is_fixed_pz"]; + edge.relative_pose_tb.fi = edge_json["fi"]; + edge.relative_pose_tb.ka = edge_json["ka"]; + edge.relative_pose_tb.om = edge_json["om"]; + edge.relative_pose_tb.px = edge_json["px"]; + edge.relative_pose_tb.py = edge_json["py"]; + edge.relative_pose_tb.pz = edge_json["pz"]; + edge.relative_pose_tb_weights.fi = edge_json["w_fi"]; + edge.relative_pose_tb_weights.ka = edge_json["w_ka"]; + edge.relative_pose_tb_weights.om = edge_json["w_om"]; + edge.relative_pose_tb_weights.px = edge_json["w_px"]; + edge.relative_pose_tb_weights.py = edge_json["w_py"]; + edge.relative_pose_tb_weights.pz = edge_json["w_pz"]; + edge.index_session_from = edge_json["index_session_from"]; + edge.index_session_to = edge_json["index_session_to"]; + edges.push_back(edge); + } + + std::cout << "Found " << edges.size() << "edges\nOpening done\n"; + + return true; + } + catch (std::exception& e) + { + std::cout << "can't load project settings: " << e.what() << std::endl; + return false; + } + + return true; +} + void openProject() { std::string input_file_name = ""; - input_file_name = mandeye::fd::OpenFileDialogOneFile("Load project", mandeye::fd::Project_filter); - std::cout << "Project file: '" << input_file_name << "'" << std::endl; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Open project", mandeye::fd::Project_filter); if (input_file_name.size() > 0) { @@ -3306,6 +3307,12 @@ void display() // gnss.render(session.point_clouds_container); + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + if (!is_loop_closure_gui) { Eigen::Affine3d prev_pose_manipulated = Eigen::Affine3d::Identity(); @@ -3651,12 +3658,6 @@ else } }*/ - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); - - ShowMainDockSpace(); - view_kbd_shortcuts(); if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) @@ -4255,7 +4256,7 @@ else ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background color", (float *)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float *)&bg_color, ImGuiColorEditFlags_NoInputs); ImGui::EndMenu(); } diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 45f321ea..7cb14ccf 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -1588,7 +1588,7 @@ void saveSubsession() void project_gui() { - ImGui::Begin("Single session processing"); + ImGui::Begin("Settings"); ImGui::Checkbox("Simple_gui", &simple_gui); ImGui::SameLine(); @@ -2083,6 +2083,12 @@ void display() if (session.ground_control_points.is_imgui) session.ground_control_points.imgui(session.point_clouds_container); + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + if (!session.control_points.is_imgui) { if (!is_loop_closure_gui) @@ -2292,12 +2298,6 @@ void display() } } - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); - - ShowMainDockSpace(); - view_kbd_shortcuts(); if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) @@ -2980,7 +2980,7 @@ void display() ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); ImGui::BeginDisabled(!session_loaded); { @@ -2993,7 +2993,7 @@ void display() color[2] = session.point_clouds_container.point_clouds[0].render_color[2]; } - if (ImGui::ColorEdit3("Cloud color", (float*)&color, ImGuiColorEditFlags_NoInputs)) + if (ImGui::ColorEdit3("Point cloud", (float*)&color, ImGuiColorEditFlags_NoInputs)) { for (auto& pc : session.point_clouds_container.point_clouds) {