diff --git a/.gitmodules b/.gitmodules index cb81a74b..88b769fb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -56,3 +56,9 @@ [submodule "3rdparty/vqf"] path = 3rdparty/vqf url = https://github.com/dlaidig/vqf.git +[submodule "3rdparty/xerces-c"] + path = 3rdparty/xerces-c + url = https://github.com/apache/xerces-c.git +[submodule "3rdparty/libE57Format"] + path = 3rdparty/libE57Format + url = https://github.com/asmaloney/libE57Format.git diff --git a/3rdparty/libE57Format b/3rdparty/libE57Format new file mode 160000 index 00000000..2524923c --- /dev/null +++ b/3rdparty/libE57Format @@ -0,0 +1 @@ +Subproject commit 2524923cabf0180cc559b81c13ea6c0566752a60 diff --git a/3rdparty/xerces-c b/3rdparty/xerces-c new file mode 160000 index 00000000..31b4b3a0 --- /dev/null +++ b/3rdparty/xerces-c @@ -0,0 +1 @@ +Subproject commit 31b4b3a06105dcd607db9fda9d1883ad7e489bfe diff --git a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt index 9824bf92..895e68ee 100644 --- a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt +++ b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt @@ -42,6 +42,7 @@ target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_trajectory_viewer PRIVATE calib_core core_pfd + core_e57 raylib_widgets raylib imgui_raylib diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index a7a26c1f..9b13ef02 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -52,7 +53,7 @@ static const std::vector appShortcuts = { { "", "Ctrl+O", "Select LIO result directory" }, { "", "Ctrl+Shift+O", "Select CAMERA_0 directory" }, { "", "Ctrl+Shift+C", "Open calibration" }, - { "", "Ctrl+S", "Export colored point cloud" }, + { "", "Ctrl+S", "Export colored point cloud (LAS/LAZ)" }, { "Camera", "F", "Front view" }, { "", "B", "Back view" }, { "", "L", "Left view" }, @@ -218,6 +219,21 @@ struct AppState char cameraBuf[512] = {}; char exportBuf[512] = "colored.laz"; std::vector exportCloud; + + // One entry per loaded LIO chunk ("scan_lio_N"), pointing at a contiguous + // [begin, begin+count) slice of exportCloud. `pose` is the chunk's MRP + // correction transform (identity when there is no session_poses.mrp). Used + // by the "Save session as E57" export to keep the segments as separate + // Data3D blocks instead of one collapsed cloud. + struct ExportSegment + { + std::string name; + Eigen::Affine3f pose = Eigen::Affine3f::Identity(); + size_t begin = 0; + size_t count = 0; + }; + std::vector exportSegments; + std::string status; // ── ROS 2 export ────────────────────────────────────────────────────────── @@ -372,6 +388,7 @@ static void loadSession(AppState& s) s.traj.poses.clear(); s.imageTsNs.clear(); s.exportCloud.clear(); + s.exportSegments.clear(); s.cloud.unload(); loadImages(s); @@ -434,6 +451,7 @@ static void loadSession(AppState& s) static void loadCloud(AppState& s) { s.exportCloud.clear(); + s.exportSegments.clear(); s.cloud.unload(); fs::path d(s.sessionBuf); @@ -595,6 +613,8 @@ static void loadCloud(AppState& s) int nImgs = (int)chunkImgs.size(); + const size_t segBegin = s.exportCloud.size(); + // ── step 4: colorize each point ───────────────────────────────────── // chunkImgs is sorted by ts (imageTsNs was sorted) // For each point: find nearest image by pt.ts_ns, expand outward until @@ -771,6 +791,12 @@ static void loadCloud(AppState& s) sumZ += pw.z(); cnt++; } + + // Record this chunk as an export segment (world-frame slice of + // exportCloud + its MRP correction pose). + if (s.exportCloud.size() > segBegin) + s.exportSegments.push_back({ key, M ? *M : Eigen::Affine3f::Identity(), segBegin, s.exportCloud.size() - segBegin }); + // chunkImgs and their cv::Mat memory are released here } s.useImageColor = canColor && (coloredChunks > 0); @@ -958,6 +984,110 @@ static void exportLAZ(AppState& s) s.status = "Exported " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; } +// E57 counterpart of exportLAZ(): one Data3D block, points already in world +// coordinates (identity pose), RGB + intensity + per-point timestamp. +static void exportE57(AppState& s) +{ + if (s.exportCloud.empty()) + { + s.status = "No cloud to export"; + return; + } + + std::vector pts; + std::vector cols; + std::vector inten; + std::vector ts; + pts.reserve(s.exportCloud.size()); + cols.reserve(s.exportCloud.size()); + inten.reserve(s.exportCloud.size()); + ts.reserve(s.exportCloud.size()); + for (const auto& p : s.exportCloud) + { + pts.emplace_back(p.x, p.y, p.z); + cols.emplace_back(p.r / 255.0, p.g / 255.0, p.b / 255.0); + inten.push_back(static_cast(std::min(1.f, std::max(0.f, p.intensity)) * 65535.f)); + ts.push_back(static_cast(p.ts_ns) * 1e-9); + } + + mandeye::e57io::E57WriteScan scan; + scan.name = "colored_cloud"; + scan.description = std::string("HDMapping ") + HDMAPPING_VERSION_STRING + " camera_lidar_trajectory_viewer"; + scan.points = &pts; + scan.colors = &cols; + scan.intensities = &inten; + scan.timestamps = &ts; + + std::string err; + if (mandeye::e57io::save_e57(s.exportBuf, { scan }, err)) + s.status = "Exported " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; + else + s.status = std::string("Export failed: ") + err; +} + +// Save the colored cloud as a *session*: one E57 Data3D block per loaded LIO +// chunk ("scan_lio_N"), NOT one collapsed cloud. Each block holds that +// segment's points in its own frame with the chunk's MRP correction as the +// block pose (identity when there is no session_poses.mrp), so the result +// re-opens as a multi-scan session (e.g. in step 2). +static void exportE57Session(AppState& s) +{ + if (s.exportSegments.empty()) + { + s.status = "No segments to export (load a session cloud first)"; + return; + } + + const std::string description = std::string("HDMapping ") + HDMAPPING_VERSION_STRING + " camera_lidar_trajectory_viewer segment"; + + const size_t nSeg = s.exportSegments.size(); + std::vector> segPts(nSeg), segCols(nSeg); + std::vector> segInten(nSeg); + std::vector> segTs(nSeg); + std::vector scans; + scans.reserve(nSeg); + + for (size_t si = 0; si < nSeg; si++) + { + const auto& seg = s.exportSegments[si]; + const bool identityPose = seg.pose.isApprox(Eigen::Affine3f::Identity()); + const Eigen::Affine3d inv = seg.pose.inverse().cast(); + + segPts[si].reserve(seg.count); + segCols[si].reserve(seg.count); + segInten[si].reserve(seg.count); + segTs[si].reserve(seg.count); + + const size_t end = std::min(seg.begin + seg.count, s.exportCloud.size()); + for (size_t k = seg.begin; k < end; k++) + { + const ColorPt& p = s.exportCloud[k]; + const Eigen::Vector3d world(p.x, p.y, p.z); + segPts[si].push_back(identityPose ? world : (inv * world)); + segCols[si].emplace_back(p.r / 255.0, p.g / 255.0, p.b / 255.0); + segInten[si].push_back(static_cast(std::min(1.f, std::max(0.f, p.intensity)) * 65535.f)); + segTs[si].push_back(static_cast(p.ts_ns) * 1e-9); + } + + mandeye::e57io::E57WriteScan sc; + sc.name = seg.name; + sc.description = description; + sc.points = &segPts[si]; + sc.colors = &segCols[si]; + sc.intensities = &segInten[si]; + sc.timestamps = &segTs[si]; + sc.pose = seg.pose.cast(); + scans.push_back(sc); + } + + std::string err; + if (mandeye::e57io::save_e57(s.exportBuf, scans, err)) + s.status = + "Exported session: " + std::to_string(nSeg) + " segment(s), " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; + else + s.status = std::string("Export failed: ") + err; +} + // ── File actions ───────────────────────────────────────────────────────────── // Factored out so the File menu items and their keyboard shortcuts (in the // main loop below) call the exact same code, matching the openSession()-style @@ -1009,10 +1139,10 @@ static void handleDroppedPath(AppState& s, const std::string& path) } } -static void actionExportColoredPointCloud(AppState& s) +static void actionExportColoredLAZ(AppState& s) { - std::string defaultName = fs::path(s.exportBuf).filename().string(); - std::string path = mandeye::fd::SaveFileDialog("Export colored point cloud", mandeye::fd::LazFilter, ".laz", defaultName); + std::string defaultName = fs::path(s.exportBuf).replace_extension(".laz").filename().string(); + std::string path = mandeye::fd::SaveFileDialog("Export colored point cloud (LAS/LAZ)", mandeye::fd::LazFilter, ".laz", defaultName); if (!path.empty()) { setBuf(s.exportBuf, sizeof(s.exportBuf), path); @@ -1020,6 +1150,28 @@ static void actionExportColoredPointCloud(AppState& s) } } +static void actionExportColoredE57(AppState& s) +{ + std::string defaultName = fs::path(s.exportBuf).replace_extension(".e57").filename().string(); + std::string path = mandeye::fd::SaveFileDialog("Export colored point cloud (E57)", mandeye::fd::E57_filter, ".e57", defaultName); + if (!path.empty()) + { + setBuf(s.exportBuf, sizeof(s.exportBuf), path); + exportE57(s); + } +} + +static void actionExportSessionE57(AppState& s) +{ + std::string defaultName = fs::path(s.exportBuf).replace_extension(".e57").filename().string(); + std::string path = mandeye::fd::SaveFileDialog("Save session as E57 (segments)", mandeye::fd::E57_filter, ".e57", defaultName); + if (!path.empty()) + { + setBuf(s.exportBuf, sizeof(s.exportBuf), path); + exportE57Session(s); + } +} + static void actionSelectRosOutputDir(AppState& s) { setBuf(s.rosOutBuf, sizeof(s.rosOutBuf), mandeye::fd::SelectFolder("Select ROS 2 bag output directory")); @@ -1473,7 +1625,7 @@ int main(int argc, char* argv[]) if (ctrlDown && shiftDown && IsKeyPressed(KEY_C)) actionOpenCalibration(s); if (ctrlDown && IsKeyPressed(KEY_S)) - actionExportColoredPointCloud(s); + actionExportColoredLAZ(s); if (!ctrlDown && IsKeyPressed(KEY_P)) s.showPath = !s.showPath; @@ -1675,8 +1827,14 @@ int main(int argc, char* argv[]) if (ImGui::MenuItem("Open Calibration...", "Ctrl+Shift+C")) actionOpenCalibration(s); ImGui::Separator(); - if (ImGui::MenuItem("Export Colored Point Cloud...", "Ctrl+S")) - actionExportColoredPointCloud(s); + if (ImGui::MenuItem("Export Colored Point Cloud (LAS/LAZ)...", "Ctrl+S")) + actionExportColoredLAZ(s); + if (ImGui::MenuItem("Export Colored Point Cloud (E57)...")) + actionExportColoredE57(s); + if (ImGui::MenuItem("Save Session as E57 (segments)...")) + actionExportSessionE57(s); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("One E57 Data3D block per LIO segment (scan_lio_N) instead of a single merged cloud"); ImGui::Separator(); if (ImGui::MenuItem("Select ROS 2 Bag Output Directory...")) actionSelectRosOutputDir(s); @@ -1921,10 +2079,18 @@ int main(int argc, char* argv[]) if (ImGui::CollapsingHeader("Export", ImGuiTreeNodeFlags_DefaultOpen)) { ImGui::PushItemWidth(-1); - ImGui::Text("Output file (.laz / .las):"); + ImGui::Text("Output file:"); ImGui::InputText("##out", s.exportBuf, sizeof(s.exportBuf)); if (ImGui::Button("Export colored LAZ", ImVec2(-1, 0))) - exportLAZ(s); + actionExportColoredLAZ(s); + if (ImGui::Button("Export colored E57", ImVec2(-1, 0))) + actionExportColoredE57(s); + ImGui::BeginDisabled(s.exportSegments.empty()); + if (ImGui::Button("Save session E57 (segments)", ImVec2(-1, 0))) + actionExportSessionE57(s); + ImGui::EndDisabled(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("One E57 Data3D block per LIO segment (scan_lio_N)"); if (!s.exportCloud.empty()) ImGui::TextDisabled("%d pts ready to export", (int)s.exportCloud.size()); ImGui::PopItemWidth(); 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 ddbf5046..f7111ff6 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 @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -81,6 +82,7 @@ #include #include #include +#include #include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" @@ -2483,44 +2485,31 @@ std::string saveSession() } } -void openLaz(bool fillInSession) +// Shared tail of openLaz()/openE57(): once session.point_clouds_container is +// populated, wire up the viewer state and -- when fillInSession is set -- +// materialize a fresh on-disk session (result folder + per-scan .laz + +// trajectory_lio_*.csv). `sourceDir` is only used for the window title. +// +// NOTE: the fillInSession branch mean-centres every cloud, which only makes +// sense for las/laz where the clouds have no pose. openE57() always passes +// fillInSession = false (E57 scans keep their embedded poses and never write a +// session folder) and does its own in-memory fill. +void finalizeScanSession(const std::string& sourceDir, bool fillInSession) { - session.point_clouds_container.point_clouds.clear(); - std::vector input_file_names; - input_file_names = mandeye::fd::OpenFileDialog("Load las/laz files", mandeye::fd::LAS_LAZ_filter, true); - if (input_file_names.size() > 0) - { - session.working_directory = fs::path(input_file_names[0]).parent_path().string(); - - spdlog::info("Creating session from las/laz files:"); - for (size_t i = 0; i < input_file_names.size(); i++) - spdlog::info("{}", input_file_names[i]); - - if (!session.point_clouds_container.load_whu_tls( - input_file_names, - tls_registration.is_decimate, - tls_registration.bucket_x, - tls_registration.bucket_y, - tls_registration.bucket_z, - tls_registration.calculate_offset, - session.load_cache_mode)) - spdlog::error("Error loading session! Check input files laz/las"); - else - spdlog::info("Loaded: {} point_clouds", session.point_clouds_container.point_clouds.size()); - - session_loaded = true; - index_begin = 0; - index_end = session.point_clouds_container.point_clouds.size() - 1; + session_loaded = true; + index_begin = 0; + index_end = session.point_clouds_container.point_clouds.size() - 1; - std::string newTitle = winTitle + " - " + fs::path(input_file_names[0]).parent_path().string(); - SetWindowTitle(newTitle.c_str()); + std::string newTitle = winTitle + " - " + sourceDir; + SetWindowTitle(newTitle.c_str()); - for (const auto& pc : session.point_clouds_container.point_clouds) - session_total_number_of_points += pc.points_local.size(); + for (const auto& pc : session.point_clouds_container.point_clouds) + session_total_number_of_points += pc.points_local.size(); - session_dims = session.point_clouds_container.compute_point_cloud_dimension(); + session_dims = session.point_clouds_container.compute_point_cloud_dimension(); - if (fillInSession) + if (fillInSession && !session.point_clouds_container.point_clouds.empty()) + { { int counter = 1; Eigen::Vector3d mean(session.point_clouds_container.point_clouds[0].points_local[0]); @@ -2575,7 +2564,9 @@ void openLaz(bool fillInSession) pc.pose = pose_tait_bryan_from_affine_matrix(m); } + } + { std::string session_fn = get_next_result_path(session.working_directory).string(); std::filesystem::create_directory(session_fn); @@ -2652,6 +2643,294 @@ void openLaz(bool fillInSession) } } +void openLaz(bool fillInSession) +{ + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load las/laz files", mandeye::fd::LAS_LAZ_filter, true); + if (input_file_names.size() == 0) + return; // dialog cancelled -- leave any already-loaded session untouched + + session.point_clouds_container.point_clouds.clear(); + session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + + spdlog::info("Creating session from las/laz files:"); + for (size_t i = 0; i < input_file_names.size(); i++) + spdlog::info("{}", input_file_names[i]); + + if (!session.point_clouds_container.load_whu_tls( + input_file_names, + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset, + session.load_cache_mode)) + spdlog::error("Error loading session! Check input files laz/las"); + else + spdlog::info("Loaded: {} point_clouds", session.point_clouds_container.point_clouds.size()); + + finalizeScanSession(fs::path(input_file_names[0]).parent_path().string(), fillInSession); +} + +void openE57(bool fillInSession) +{ + std::vector input_file_names = mandeye::fd::OpenFileDialog("Load e57 files", mandeye::fd::E57_filter, true); + if (input_file_names.size() == 0) + return; // dialog cancelled -- leave any already-loaded session untouched + + spdlog::info("Creating session from e57 files:"); + for (const auto& fn : input_file_names) + spdlog::info("{}", fn); + + // Build into a local list first so a full failure leaves the current session intact. + std::vector loaded; + for (const auto& fn : input_file_names) + { + std::vector scans; + std::string err; + if (!mandeye::e57io::load_e57(fn, scans, err)) + { + spdlog::error("Failed to load e57 '{}': {}", fn, err); + [[maybe_unused]] pfd::message message( + "E57 load error", "Could not read:\n" + fn + "\n\n" + err, pfd::choice::ok, pfd::icon::error); + message.result(); + continue; + } + + const std::string stem = fs::path(fn).stem().string(); + std::string abs_src; + try + { + abs_src = fs::absolute(fn).string(); + } catch (const std::exception&) + { + abs_src = fn; + } + + for (size_t si = 0; si < scans.size(); si++) + { + auto& scan = scans[si]; + + PointCloud pc; + // Name the cloud so it exports to a sensible .laz in fillInSession mode. + pc.file_name = scans.size() > 1 ? (stem + "_" + std::to_string(si) + ".laz") : (stem + ".laz"); + // Provenance for "Update e57 poses". + pc.e57_source_path = abs_src; + pc.e57_scan_index = static_cast(si); + pc.points_local = std::move(scan.points); + pc.intensities = std::move(scan.intensities); + pc.colors = std::move(scan.colors); + pc.timestamps = std::move(scan.timestamps); + + // exportLaz() and PointCloud::decimate() index intensities/timestamps + // per point, so pad the ones the E57 scan didn't provide. + if (pc.intensities.size() != pc.points_local.size()) + pc.intensities.assign(pc.points_local.size(), 0); + if (pc.timestamps.size() != pc.points_local.size()) + pc.timestamps.assign(pc.points_local.size(), 0.0); + + pc.m_initial_pose = scan.pose; + pc.m_pose = scan.pose; + pc.m_pose_temp = scan.pose; + pc.pose = pose_tait_bryan_from_affine_matrix(scan.pose); + pc.gui_translation[0] = static_cast(pc.pose.px); + pc.gui_translation[1] = static_cast(pc.pose.py); + pc.gui_translation[2] = static_cast(pc.pose.pz); + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + + if (tls_registration.is_decimate && pc.points_local.size() > 0) + pc.decimate(tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z); + + loaded.push_back(std::move(pc)); + } + } + + if (loaded.empty()) + { + spdlog::error("No scans loaded from the selected e57 file(s)"); + return; + } + + session.point_clouds_container.point_clouds = std::move(loaded); + session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + spdlog::info( + "Loaded: {} point_clouds from {} e57 file(s)", session.point_clouds_container.point_clouds.size(), input_file_names.size()); + + // E57 scans carry their own poses, so the session is already complete in + // memory. When "Fill in session" is set, just add the identity local + // trajectory node the fill flow expects -- but never write a session/result + // folder to disk here. The user saves explicitly via "Save session as". + if (fillInSession) + { + for (auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.local_trajectory.empty()) + continue; + PointCloud::LocalTrajectoryNode node; + node.imu_diff_angle_om_fi_ka_deg = { 0, 0, 0 }; + node.imu_om_fi_ka = { 0, 0, 0 }; + node.m_pose = Eigen::Affine3d::Identity(); + node.timestamps = { 0, 0 }; + pc.local_trajectory.push_back(node); + } + } + + finalizeScanSession(fs::path(input_file_names[0]).parent_path().string(), false /* never create session files for e57 */); +} + +// Write the current (registered) poses of E57-imported scans back into the +// originating .e57 file(s), replacing each Data3D `pose` element. All other +// content (points, colors, intensity, line groups, 2D images) is copied +// verbatim. Only scans that still carry their e57 provenance are considered. +void updateE57Poses() +{ + // file -> { Data3D index -> refined pose } + std::map> by_file; + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (pc.e57_source_path.empty() || pc.e57_scan_index < 0) + continue; + // openE57 keeps points scan-local and never applies a session offset, so + // the refined m_pose is already the file-level Data3D pose to write back. + by_file[pc.e57_source_path][pc.e57_scan_index] = pc.m_pose; + } + + if (by_file.empty()) + { + [[maybe_unused]] pfd::message m( + "Update e57 poses", "No scans in this session were loaded from an e57 file.", pfd::choice::ok, pfd::icon::warning); + m.result(); + return; + } + + const pfd::button choice = pfd::message( + "Update e57 poses", + "Write updated Data3D poses into " + std::to_string(by_file.size()) + + " e57 file(s)?\n\n" + "Yes - overwrite the original file(s) in place\n" + "No - write copies as _updated.e57\n" + "Cancel - abort", + pfd::choice::yes_no_cancel, + pfd::icon::question) + .result(); + + if (choice == pfd::button::cancel) + return; + const bool in_place = (choice == pfd::button::yes); + + int ok = 0; + int failed = 0; + std::string errors; + for (const auto& [src, poses] : by_file) + { + const fs::path srcp(src); + const fs::path tmp = srcp.parent_path() / (srcp.stem().string() + ".e57.tmp"); + + std::string err; + if (!mandeye::e57io::rewrite_e57_poses(src, tmp.string(), poses, err)) + { + failed++; + errors += "\n- " + src + "\n " + err; + std::error_code ec; + fs::remove(tmp, ec); + continue; + } + + const fs::path dst = in_place ? srcp : srcp.parent_path() / (srcp.stem().string() + "_updated.e57"); + std::error_code ec; + fs::rename(tmp, dst, ec); + if (ec) + { + ec.clear(); + fs::copy_file(tmp, dst, fs::copy_options::overwrite_existing, ec); + std::error_code ec2; + fs::remove(tmp, ec2); + } + if (ec) + { + failed++; + errors += "\n- " + src + "\n could not place result: " + ec.message(); + continue; + } + + ok++; + spdlog::info("Update e57 poses: wrote '{}' ({} scan pose(s))", dst.string(), poses.size()); + } + + [[maybe_unused]] pfd::message summary( + "Update e57 poses", + std::to_string(ok) + " file(s) updated, " + std::to_string(failed) + " failed." + + (errors.empty() ? std::string() : ("\n\nErrors:" + errors)), + pfd::choice::ok, + failed > 0 ? pfd::icon::error : pfd::icon::info); + summary.result(); +} + +// Write the whole session out as one multi-scan .e57 file: one Data3D block per +// point cloud, points in their scan-local frame, each block carrying the current +// (registered) pose. Independent of the "never write files on e57 load" rule -- +// this is an explicit, user-driven export. +void saveSessionAsE57() +{ + if (session.point_clouds_container.point_clouds.empty()) + { + [[maybe_unused]] pfd::message m( + "Save session as e57", "The session has no point clouds to save.", pfd::choice::ok, pfd::icon::warning); + m.result(); + return; + } + + std::string default_name = "session.e57"; + if (!session_file_name.empty()) + default_name = fs::path(session_file_name).replace_extension(".e57").string(); + + const std::string out = mandeye::fd::SaveFileDialog("Save session as e57", mandeye::fd::E57_filter, ".e57", default_name); + if (out.empty()) + return; + + const std::string description = std::string("HDMapping ") + HDMAPPING_VERSION_STRING + " session"; + + std::vector scans; + scans.reserve(session.point_clouds_container.point_clouds.size()); + for (const auto& pc : session.point_clouds_container.point_clouds) + { + mandeye::e57io::E57WriteScan s; + s.name = fs::path(pc.file_name).stem().string(); + s.description = description; + s.points = &pc.points_local; + if (pc.intensities.size() == pc.points_local.size()) + s.intensities = &pc.intensities; + if (pc.colors.size() == pc.points_local.size()) + s.colors = &pc.colors; + if (pc.timestamps.size() == pc.points_local.size()) + s.timestamps = &pc.timestamps; + + // File-level pose = session offset (a pure translation) composed with the + // scan's registered pose. For e57-loaded sessions the offset is zero. + s.pose = pc.m_pose; + s.pose.translation() += session.point_clouds_container.offset; + + scans.push_back(std::move(s)); + } + + std::string err; + if (mandeye::e57io::save_e57(out, scans, err)) + { + spdlog::info("Saved session as e57: '{}' ({} scans)", out, scans.size()); + [[maybe_unused]] pfd::message m( + "Save session as e57", "Saved " + std::to_string(scans.size()) + " scan(s) to:\n" + out, pfd::choice::ok, pfd::icon::info); + m.result(); + } + else + { + spdlog::error("Save session as e57 failed: {}", err); + [[maybe_unused]] pfd::message m("Save session as e57", "Failed:\n" + err, pfd::choice::ok, pfd::icon::error); + m.result(); + } +} + void saveSubsession() { int inx_begin = 0; @@ -4463,6 +4742,11 @@ void display() if (ImGui::IsItemHovered()) ImGui::SetTooltip("Create session from las/laz file(s)"); + if (ImGui::MenuItem("Open e57")) + openE57(fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Create session from e57 file(s); embedded per-scan poses are used as initial poses"); + ImGui::EndPopup(); } @@ -4474,6 +4758,57 @@ void display() { if (ImGui::BeginMenu("File")) { + if (ImGui::BeginMenu("Open")) + { + ImGui::MenuItem("Calculate_offset", nullptr, &tls_registration.calculate_offset); + ImGui::MenuItem("Fill in session", nullptr, &fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Fill in data for trajectory and pose to create complete session"); + + ImGui::Separator(); + + if (ImGui::MenuItem("Open session")) + openSession(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Replace the current session with another one (Ctrl+O)"); + + if (ImGui::MenuItem("Open las/laz")) + openLaz(fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Replace the current session with one created from las/laz file(s)"); + + if (ImGui::MenuItem("Open e57")) + openE57(fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Replace the current session with one created from e57 file(s); " + "embedded per-scan poses are used as initial poses"); + + ImGui::EndMenu(); + } + + { + bool has_e57 = false; + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.e57_source_path.empty()) + { + has_e57 = true; + break; + } + } + ImGui::BeginDisabled(!has_e57); + if (ImGui::MenuItem("Update e57 poses")) + updateE57Poses(); + ImGui::EndDisabled(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Write the current registered poses back into the Data3D headers " + "of the source e57 file(s)"); + } + + ImGui::Separator(); + if (ImGui::MenuItem("Save session as", "Ctrl+S")) saveSession(); if (ImGui::IsItemHovered()) @@ -4489,6 +4824,13 @@ void display() //} // ImGui::EndDisabled(); + if (ImGui::MenuItem("Save session as e57")) + saveSessionAsE57(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Write every point cloud of the session to one multi-scan .e57 file, " + "each scan carrying its current (registered) pose"); + ImGui::Separator(); if (ImGui::BeginMenu("Save all marked scans")) { diff --git a/cmake/FindXercesC.cmake b/cmake/FindXercesC.cmake new file mode 100644 index 00000000..2e248170 --- /dev/null +++ b/cmake/FindXercesC.cmake @@ -0,0 +1,37 @@ +# Shim FindXercesC module for HDMapping. +# +# HDMapping vendors Apache Xerces-C as a git submodule (3rdparty/xerces-c) and +# builds it from source via add_subdirectory() in cmake/dependencies.cmake. +# libE57Format (3rdparty/libE57Format) calls `find_package( XercesC REQUIRED )` +# expecting a system-installed Xerces; this shim redirects that lookup to the +# in-tree `xerces-c` target instead so no system package is needed. +# +# cmake/ is first on CMAKE_MODULE_PATH (see top-level CMakeLists.txt), so this +# file wins over CMake's built-in FindXercesC when libE57Format is configured. + +if(TARGET xerces-c) + if(NOT TARGET XercesC::XercesC) + # ALIAS can't point at a non-GLOBAL target from another directory on + # older CMake, so wrap it in an INTERFACE IMPORTED target instead. + add_library(XercesC::XercesC INTERFACE IMPORTED) + target_link_libraries(XercesC::XercesC INTERFACE xerces-c) + endif() + + set(XercesC_FOUND TRUE) + set(XERCESC_FOUND TRUE) + # Version of the vendored submodule (keep in sync with 3rdparty/xerces-c). + set(XercesC_VERSION "3.3.0") + set(XercesC_VERSION_STRING "3.3.0") + set(XercesC_LIBRARIES XercesC::XercesC) + set(XercesC_INCLUDE_DIRS "") # carried transitively by the xerces-c target + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(XercesC + REQUIRED_VARS XercesC_LIBRARIES + VERSION_VAR XercesC_VERSION) +else() + message(FATAL_ERROR + "FindXercesC shim: the bundled `xerces-c` target does not exist yet. " + "add_subdirectory(3rdparty/xerces-c) must run before anything that " + "calls find_package(XercesC) -- see cmake/dependencies.cmake.") +endif() diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 4baa3346..7a81177c 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -75,6 +75,73 @@ else() message(STATUS "LASzip include dir: ${LASZIP_INCLUDE_DIR}, LASzip library: ${LASZIP_LIBRARY}") endif() +# E57 - Apache Xerces-C + libE57Format, both vendored as git submodules and +# built from source (no system packages). Xerces-C provides the XML parser +# libE57Format needs; cmake/FindXercesC.cmake redirects libE57Format's +# find_package(XercesC) to the in-tree `xerces-c` target defined here. +# +# NOTE: xerces-c's bundled tests/samples are kept out of CTest -- see the +# add_subdirectory() override further down. libE57Format's own tests are +# disabled via E57_BUILD_TEST=OFF. +# +# xerces-c/cmake/XercesDLL.cmake unconditionally runs +# set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries") +# With no prior cache entry that creates BUILD_SHARED_LIBS=ON in the cache and +# flips EVERY dependency added afterwards (vqf, Fusion, glad, plycpp, ...) to +# a DLL with no exported symbols -- on Windows that means no import .lib and a +# project-wide link failure. Pin a cache entry to OFF up front so that set() +# becomes a no-op, then remove it so nothing else is affected. HDMapping builds +# all dependencies statically, so an existing BUILD_SHARED_LIBS is not +# expected; save and restore it anyway. +if(DEFINED CACHE{BUILD_SHARED_LIBS}) + set(_hd_bsl_prev "$CACHE{BUILD_SHARED_LIBS}") + set(_hd_bsl_restore ON) +else() + set(_hd_bsl_restore OFF) +endif() +set(BUILD_SHARED_LIBS OFF CACHE BOOL "HDMapping links Xerces-C + libE57Format statically" FORCE) + +# Keep xerces-c's bundled `tests` and `samples` subdirectories out of the +# configure entirely: they unconditionally call enable_testing() + add_test() +# for ~80 sample-driven cases whose executables we never build +# (EXCLUDE_FROM_ALL), which then fail `ctest` in CI. Override add_subdirectory() +# to drop those two leaves while the _hd_skip_test_subdirs guard is set; the +# builtin stays reachable as _add_subdirectory() and the guard is cleared right +# after so every later add_subdirectory() (libE57Format, core, apps, HDMapping's +# own tests) behaves normally. +set(_hd_skip_test_subdirs ON) +macro(add_subdirectory _hd_dir) + get_filename_component(_hd_leaf "${_hd_dir}" NAME) + if(_hd_skip_test_subdirs AND (_hd_leaf STREQUAL "tests" OR _hd_leaf STREQUAL "samples")) + message(STATUS "Skipping xerces-c '${_hd_leaf}' subdirectory (E57 tests disabled)") + else() + _add_subdirectory("${_hd_dir}" ${ARGN}) + endif() +endmacro() + +# Xerces-C transcoder/netaccessor/message-loader defaults are the platform +# native, dependency-free choices (macOS: macosunicodeconverter/cfurl, +# Windows: windows/winsock, Linux: iconv/socket) -- no ICU, no libcurl. +add_subdirectory(${THIRDPARTY_DIRECTORY}/xerces-c ${CMAKE_BINARY_DIR}/3rdparty/xerces-c EXCLUDE_FROM_ALL) + +set(_hd_skip_test_subdirs OFF) + +# libE57Format: static lib target `E57Format`, headers at libE57Format/include. +set(E57_BUILD_TEST OFF CACHE BOOL "" FORCE) +set(E57_RELEASE_LTO OFF CACHE BOOL "" FORCE) # don't force LTO into the wider build +if(WIN32) + set(USING_STATIC_XERCES ON CACHE BOOL "" FORCE) # adds XERCES_STATIC_LIBRARY define +endif() +add_subdirectory(${THIRDPARTY_DIRECTORY}/libE57Format ${CMAKE_BINARY_DIR}/3rdparty/libE57Format EXCLUDE_FROM_ALL) +set(LIBE57FORMAT_INCLUDE_DIR ${THIRDPARTY_DIRECTORY}/libE57Format/include) + +if(_hd_bsl_restore) + set(BUILD_SHARED_LIBS "${_hd_bsl_prev}" CACHE BOOL "Build shared libraries" FORCE) +else() + unset(BUILD_SHARED_LIBS CACHE) +endif() +message(STATUS "Using bundled Xerces-C + libE57Format (static) for E57 support") + # ============================================================================ # Computer Vision & Geospatial Libraries (Pre-downloaded) # ============================================================================ diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 0140c773..9511b62d 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -69,6 +69,17 @@ target_include_directories(core_pfd PUBLIC include) target_include_directories(core_pfd PRIVATE ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master) set_target_properties(core_pfd PROPERTIES POSITION_INDEPENDENT_CODE ON) +# core_e57 -- E57 point-cloud read/write (mandeye::e57io, wrapping libE57Format +# + Xerces-C). Split out like core_pfd so the camera_lidar_* apps (which link +# calib_core, not core) can use E57 export without pulling in core_math/PROJ/ +# the rest. Linked PUBLIC into core/core_no_gui below, so existing core +# consumers still get mandeye::e57io transparently. +add_library(core_e57 STATIC src/e57_utils.cpp) +target_include_directories(core_e57 PUBLIC include ${LIBE57FORMAT_INCLUDE_DIR}) +target_include_directories(core_e57 PRIVATE ${EIGEN3_INCLUDE_DIR}) +target_link_libraries(core_e57 PUBLIC E57Format PRIVATE spdlog::spdlog) +set_target_properties(core_e57 PROPERTIES POSITION_INDEPENDENT_CODE ON) + function(add_core_target target_name with_gui) if(${with_gui}) set(SOURCES ${CORE_BASE_SOURCES} ${CORE_GUI_SOURCES}) @@ -80,7 +91,9 @@ function(add_core_target target_name with_gui) add_library(${target_name} STATIC ${SOURCES}) target_compile_definitions(${target_name} PRIVATE ${DEFINES}) - target_link_libraries(${target_name} PRIVATE core_math ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} PROJ::proj spdlog::spdlog vqf Fusion wgs84_do_puwg92 plycpp WGS84toCartesian) + # core_e57 (PUBLIC) provides mandeye::e57io + pulls E57Format/Xerces-C onto + # the final link line for consumers that call it. + target_link_libraries(${target_name} PRIVATE core_math ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} PROJ::proj spdlog::spdlog vqf Fusion wgs84_do_puwg92 plycpp WGS84toCartesian PUBLIC core_e57) target_include_directories(${target_name} PRIVATE include ${EIGEN3_INCLUDE_DIR} diff --git a/core/include/Core/e57_utils.h b/core/include/Core/e57_utils.h new file mode 100644 index 00000000..80aa17c6 --- /dev/null +++ b/core/include/Core/e57_utils.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include + +#include + +// NOTE: namespace is `e57io`, not `e57`, to avoid colliding with libE57Format's +// own top-level `::e57` namespace inside the implementation. +namespace mandeye::e57io +{ + // One E57 `Data3D` block (a single scan) decoded into HDMapping-friendly buffers. + // Points are kept in the scan-local coordinate frame; `pose` places that frame in + // the file-level frame (from the Data3D `pose` element, identity when absent). + struct E57Scan + { + std::string name; + std::vector points; // cartesian, scan-local, meters + std::vector intensities; // empty when the scan has no intensity + std::vector colors; // 0..1 RGB, empty when the scan has no color + std::vector timestamps; // seconds, empty when the scan has no timestamps + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + }; + + // Reads every Data3D block of `path`. Returns false and fills `error` on failure + // (file missing, not an E57, corrupt, unreadable). Scans with zero valid points are + // skipped. Spherical-only scans are converted to cartesian. + bool load_e57(const std::string& path, std::vector& scans, std::string& error); + + // Copies `src_path` to `dst_path` verbatim (all scans, images, point fields + // and attributes preserved) except that the `pose` of each Data3D block whose + // index appears in `new_poses` is replaced with the given file-level transform. + // `src_path` and `dst_path` must differ. Returns false and fills `error` on + // failure, leaving `dst_path` in an unspecified state (callers write to a + // temp file and only swap it in on success). + bool rewrite_e57_poses( + const std::string& src_path, const std::string& dst_path, const std::map& new_poses, std::string& error); + + // One scan to write out with save_e57(). Vectors are borrowed (not copied), + // so they must outlive the save_e57() call. `points` is required and holds + // scan-local cartesian coordinates; `pose` places that frame in the + // file-level frame. `intensities` (0..65535), `colors` (0..1 RGB per + // channel) and `timestamps` are optional -- pass nullptr or a vector whose + // size differs from `points` to omit that field. + struct E57WriteScan + { + std::string name; + std::string description; // optional, free text stored in the Data3D block + const std::vector* points = nullptr; + const std::vector* intensities = nullptr; + const std::vector* colors = nullptr; + const std::vector* timestamps = nullptr; + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + }; + + // Writes `scans` as a fresh multi-block E57 file at `dst_path` (overwriting + // any existing file). Each scan becomes one Data3D block carrying its pose. + // Returns false and fills `error` on failure. + bool save_e57(const std::string& dst_path, const std::vector& scans, std::string& error); +} // namespace mandeye::e57io diff --git a/core/include/Core/pfd_wrapper.hpp b/core/include/Core/pfd_wrapper.hpp index 3665d317..87d8a2f2 100644 --- a/core/include/Core/pfd_wrapper.hpp +++ b/core/include/Core/pfd_wrapper.hpp @@ -12,6 +12,7 @@ namespace mandeye::fd const std::vector LAS_LAZ_filter = { "LASzip file (*.laz)", "*.laz", "LAS file (*.las)", "*.las", "All files", "*" }; const std::vector LazFilter = { "LAS/LAZ files (*.laz)", "*.las *.laz" }; + const std::vector E57_filter = { "E57 point cloud (*.e57)", "*.e57", "All files", "*" }; const std::vector ImageFilter = { "Image files (*.bmp, *.jpg, *.jpeg, *.png)", "*.bmp *.jpg *.jpeg *.png", "All files", "*" }; diff --git a/core/include/Core/point_cloud.h b/core/include/Core/point_cloud.h index e3e9846c..047ef811 100644 --- a/core/include/Core/point_cloud.h +++ b/core/include/Core/point_cloud.h @@ -88,6 +88,11 @@ class PointCloud std::vector index_pairs; std::vector buckets; std::string file_name; + // Provenance for scans imported from an E57 file (see openE57 in step 2). + // Not serialized to session JSON; used by "Update e57 poses" to write the + // refined m_pose back into the originating Data3D block. + std::string e57_source_path; + int e57_scan_index = -1; std::vector points_local; std::vector normal_vectors_local; std::vector colors; diff --git a/core/src/e57_utils.cpp b/core/src/e57_utils.cpp new file mode 100644 index 00000000..185b159d --- /dev/null +++ b/core/src/e57_utils.cpp @@ -0,0 +1,700 @@ +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace mandeye::e57io +{ + namespace + { + constexpr size_t kChunk = 1u << 20; // points per read() call + + Eigen::Affine3d pose_from_header(const ::e57::Data3D& h) + { + const auto& r = h.pose.rotation; + const auto& t = h.pose.translation; + Eigen::Quaterniond q(r.w, r.x, r.y, r.z); + if (q.norm() < 1e-12) + q = Eigen::Quaterniond::Identity(); + q.normalize(); + + Eigen::Affine3d m = Eigen::Affine3d::Identity(); + m.linear() = q.toRotationMatrix(); + m.translation() = Eigen::Vector3d(t.x, t.y, t.z); + return m; + } + + // Maps a raw value in [lo, hi] to [0, 1]. Falls back to `assume_max` when the + // header gives no usable limits (lo == hi), which is common for color (0..255). + double normalize(double v, double lo, double hi, double assume_max) + { + if (hi > lo) + return std::clamp((v - lo) / (hi - lo), 0.0, 1.0); + if (assume_max > 0.0) + return std::clamp(v / assume_max, 0.0, 1.0); + return std::clamp(v, 0.0, 1.0); + } + + ::e57::RigidBodyTransform rbt_from_affine(const Eigen::Affine3d& m) + { + Eigen::Quaterniond q(m.rotation()); + q.normalize(); + + ::e57::RigidBodyTransform t; + t.rotation.w = q.w(); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + t.translation.x = m.translation().x(); + t.translation.y = m.translation().y(); + t.translation.z = m.translation().z(); + return t; + } + + // Chunk-sized storage for every point field a Data3D can carry, with the + // matching Data3DPointsDouble pointers wired up. One instance is bound to + // both a reader and a writer so points stream through verbatim. + struct CopyBuffers + { + std::vector cx, cy, cz, sr, sa, se, intensity, tstamp; + std::vector cr, cg, cb; + std::vector rowIdx, colIdx; + std::vector cartInv, sphInv, iInv, cInv, tInv, retIdx, retCnt; + std::vector nx, ny, nz; + + void bind(const ::e57::Data3D& h, size_t chunk, ::e57::Data3DPointsDouble& b) + { + const auto& f = h.pointFields; + auto dbl = [chunk](bool on, std::vector& v, double*& p) + { + if (on) + { + v.assign(chunk, 0.0); + p = v.data(); + } + }; + auto u16 = [chunk](bool on, std::vector& v, uint16_t*& p) + { + if (on) + { + v.assign(chunk, 0); + p = v.data(); + } + }; + auto i32 = [chunk](bool on, std::vector& v, int32_t*& p) + { + if (on) + { + v.assign(chunk, 0); + p = v.data(); + } + }; + auto i8 = [chunk](bool on, std::vector& v, int8_t*& p) + { + if (on) + { + v.assign(chunk, 0); + p = v.data(); + } + }; + auto flt = [chunk](bool on, std::vector& v, float*& p) + { + if (on) + { + v.assign(chunk, 0.0f); + p = v.data(); + } + }; + + dbl(f.cartesianXField, cx, b.cartesianX); + dbl(f.cartesianYField, cy, b.cartesianY); + dbl(f.cartesianZField, cz, b.cartesianZ); + i8(f.cartesianInvalidStateField, cartInv, b.cartesianInvalidState); + dbl(f.sphericalRangeField, sr, b.sphericalRange); + dbl(f.sphericalAzimuthField, sa, b.sphericalAzimuth); + dbl(f.sphericalElevationField, se, b.sphericalElevation); + i8(f.sphericalInvalidStateField, sphInv, b.sphericalInvalidState); + dbl(f.intensityField, intensity, b.intensity); + i8(f.isIntensityInvalidField, iInv, b.isIntensityInvalid); + u16(f.colorRedField, cr, b.colorRed); + u16(f.colorGreenField, cg, b.colorGreen); + u16(f.colorBlueField, cb, b.colorBlue); + i8(f.isColorInvalidField, cInv, b.isColorInvalid); + i32(f.rowIndexField, rowIdx, b.rowIndex); + i32(f.columnIndexField, colIdx, b.columnIndex); + i8(f.returnIndexField, retIdx, b.returnIndex); + i8(f.returnCountField, retCnt, b.returnCount); + dbl(f.timeStampField, tstamp, b.timeStamp); + i8(f.isTimeStampInvalidField, tInv, b.isTimeStampInvalid); + flt(f.normalXField, nx, b.normalX); + flt(f.normalYField, ny, b.normalY); + flt(f.normalZField, nz, b.normalZ); + } + }; + + // `pose_deltas` maps a Data3D guid to the rigid transform (new * old^-1) + // applied to that scan, so 2D images rigidly attached to a moved scan + // follow it. + void copy_image2d( + const ::e57::Reader& reader, ::e57::Writer& writer, int64_t srcIdx, const std::map& pose_deltas) + { + ::e57::Image2D ih; + if (!reader.ReadImage2D(srcIdx, ih)) + throw std::runtime_error("failed to read Image2D header " + std::to_string(srcIdx)); + + const auto delta = pose_deltas.find(ih.associatedData3DGuid); + if (delta != pose_deltas.end()) + { + const auto& r = ih.pose.rotation; + const auto& t = ih.pose.translation; + Eigen::Quaterniond q(r.w, r.x, r.y, r.z); + if (q.norm() < 1e-12) + q = Eigen::Quaterniond::Identity(); + q.normalize(); + Eigen::Affine3d img = Eigen::Affine3d::Identity(); + img.linear() = q.toRotationMatrix(); + img.translation() = Eigen::Vector3d(t.x, t.y, t.z); + + ih.pose = rbt_from_affine(delta->second * img); + } + + const int64_t dstIdx = writer.NewImage2D(ih); + + auto copyRepr = [&](::e57::Image2DProjection proj, int64_t jpegSize, int64_t pngSize, int64_t maskSize) + { + auto blob = [&](::e57::Image2DType type, int64_t size) + { + if (size <= 0) + return; + std::vector buf(static_cast(size)); + const int64_t got = reader.ReadImage2DData(srcIdx, proj, type, buf.data(), 0, size); + if (got <= 0) + throw std::runtime_error("failed to read Image2D blob"); + writer.WriteImage2DData(dstIdx, type, proj, buf.data(), 0, got); + }; + blob(::e57::ImageJPEG, jpegSize); + blob(::e57::ImagePNG, pngSize); + blob(::e57::ImageMaskPNG, maskSize); + }; + + const auto& v = ih.visualReferenceRepresentation; + const auto& p = ih.pinholeRepresentation; + const auto& s = ih.sphericalRepresentation; + const auto& c = ih.cylindricalRepresentation; + if (v.jpegImageSize || v.pngImageSize || v.imageMaskSize) + copyRepr(::e57::ProjectionVisual, v.jpegImageSize, v.pngImageSize, v.imageMaskSize); + if (p.jpegImageSize || p.pngImageSize || p.imageMaskSize) + copyRepr(::e57::ProjectionPinhole, p.jpegImageSize, p.pngImageSize, p.imageMaskSize); + if (s.jpegImageSize || s.pngImageSize || s.imageMaskSize) + copyRepr(::e57::ProjectionSpherical, s.jpegImageSize, s.pngImageSize, s.imageMaskSize); + if (c.jpegImageSize || c.pngImageSize || c.imageMaskSize) + copyRepr(::e57::ProjectionCylindrical, c.jpegImageSize, c.pngImageSize, c.imageMaskSize); + } + } // namespace + + bool load_e57(const std::string& path, std::vector& scans, std::string& error) + { + scans.clear(); + error.clear(); + + try + { + ::e57::Reader reader(path, ::e57::ReaderOptions{}); + if (!reader.IsOpen()) + { + error = "could not open E57 file: '" + path + "'"; + return false; + } + + const int64_t scan_count = reader.GetData3DCount(); + if (scan_count <= 0) + { + error = "E57 file has no Data3D blocks: '" + path + "'"; + return false; + } + + for (int64_t si = 0; si < scan_count; si++) + { + ::e57::Data3D header; + if (!reader.ReadData3D(si, header)) + { + spdlog::warn("e57: failed to read Data3D header {} in '{}', skipping", si, path); + continue; + } + + int64_t row_max = 0, col_max = 0, points_size = 0, groups_size = 0, count_size = 0; + bool column_index = false; + reader.GetData3DSizes(si, row_max, col_max, points_size, groups_size, count_size, column_index); + if (points_size <= 0) + points_size = static_cast(header.pointCount); + if (points_size <= 0) + { + spdlog::warn("e57: Data3D {} in '{}' has no points, skipping", si, path); + continue; + } + + const auto& pf = header.pointFields; + const bool has_cartesian = pf.cartesianXField && pf.cartesianYField && pf.cartesianZField; + const bool has_spherical = pf.sphericalRangeField && pf.sphericalAzimuthField && pf.sphericalElevationField; + if (!has_cartesian && !has_spherical) + { + spdlog::warn("e57: Data3D {} in '{}' has neither cartesian nor spherical coords, skipping", si, path); + continue; + } + const bool has_intensity = pf.intensityField; + const bool has_color = pf.colorRedField && pf.colorGreenField && pf.colorBlueField; + const bool has_time = pf.timeStampField; + + const size_t chunk = std::min(kChunk, static_cast(points_size)); + + std::vector cx, cy, cz, sr, sa, se, intensity, tstamp; + std::vector cr, cg, cb; + std::vector cart_invalid, sph_invalid, intensity_invalid, color_invalid; + + ::e57::Data3DPointsDouble buffers; // default ctor: does not own memory + if (has_cartesian) + { + cx.resize(chunk); + cy.resize(chunk); + cz.resize(chunk); + buffers.cartesianX = cx.data(); + buffers.cartesianY = cy.data(); + buffers.cartesianZ = cz.data(); + if (pf.cartesianInvalidStateField) + { + cart_invalid.resize(chunk); + buffers.cartesianInvalidState = cart_invalid.data(); + } + } + if (has_spherical) + { + sr.resize(chunk); + sa.resize(chunk); + se.resize(chunk); + buffers.sphericalRange = sr.data(); + buffers.sphericalAzimuth = sa.data(); + buffers.sphericalElevation = se.data(); + if (pf.sphericalInvalidStateField) + { + sph_invalid.resize(chunk); + buffers.sphericalInvalidState = sph_invalid.data(); + } + } + if (has_intensity) + { + intensity.resize(chunk); + buffers.intensity = intensity.data(); + if (pf.isIntensityInvalidField) + { + intensity_invalid.resize(chunk); + buffers.isIntensityInvalid = intensity_invalid.data(); + } + } + if (has_color) + { + cr.resize(chunk); + cg.resize(chunk); + cb.resize(chunk); + buffers.colorRed = cr.data(); + buffers.colorGreen = cg.data(); + buffers.colorBlue = cb.data(); + if (pf.isColorInvalidField) + { + color_invalid.resize(chunk); + buffers.isColorInvalid = color_invalid.data(); + } + } + if (has_time) + { + tstamp.resize(chunk); + buffers.timeStamp = tstamp.data(); + } + + E57Scan scan; + scan.name = !header.name.empty() ? header.name : ("scan_" + std::to_string(si)); + scan.pose = pose_from_header(header); + scan.points.reserve(static_cast(points_size)); + if (has_intensity) + scan.intensities.reserve(static_cast(points_size)); + if (has_color) + scan.colors.reserve(static_cast(points_size)); + if (has_time) + scan.timestamps.reserve(static_cast(points_size)); + + const double i_lo = header.intensityLimits.intensityMinimum; + const double i_hi = header.intensityLimits.intensityMaximum; + const double cr_lo = header.colorLimits.colorRedMinimum; + const double cr_hi = header.colorLimits.colorRedMaximum; + const double cg_lo = header.colorLimits.colorGreenMinimum; + const double cg_hi = header.colorLimits.colorGreenMaximum; + const double cb_lo = header.colorLimits.colorBlueMinimum; + const double cb_hi = header.colorLimits.colorBlueMaximum; + + ::e57::CompressedVectorReader vr = reader.SetUpData3DPointsData(si, chunk, buffers); + unsigned got = 0; + while ((got = vr.read()) > 0) + { + for (unsigned k = 0; k < got; k++) + { + if (!cart_invalid.empty() && cart_invalid[k] != 0) + continue; + if (!sph_invalid.empty() && sph_invalid[k] != 0) + continue; + + Eigen::Vector3d p; + if (has_cartesian) + { + p = Eigen::Vector3d(cx[k], cy[k], cz[k]); + } + else + { + const double rng = sr[k]; + const double az = sa[k]; + const double el = se[k]; + p = Eigen::Vector3d(rng * std::cos(el) * std::cos(az), rng * std::cos(el) * std::sin(az), rng * std::sin(el)); + } + if (!p.allFinite()) + continue; + + scan.points.push_back(p); + + if (has_intensity) + { + const double n = normalize(intensity[k], i_lo, i_hi, 0.0); + scan.intensities.push_back(static_cast(std::lround(n * 65535.0))); + } + if (has_color) + { + scan.colors.emplace_back( + normalize(cr[k], cr_lo, cr_hi, 255.0), + normalize(cg[k], cg_lo, cg_hi, 255.0), + normalize(cb[k], cb_lo, cb_hi, 255.0)); + } + if (has_time) + scan.timestamps.push_back(tstamp[k]); + } + } + vr.close(); + + if (scan.points.empty()) + { + spdlog::warn("e57: Data3D {} ('{}') in '{}' produced no valid points, skipping", si, scan.name, path); + continue; + } + + spdlog::info( + "e57: loaded scan '{}' ({} points{}{}{}) from '{}'", + scan.name, + scan.points.size(), + has_intensity ? ", intensity" : "", + has_color ? ", rgb" : "", + has_time ? ", time" : "", + path); + scans.push_back(std::move(scan)); + } + } catch (const ::e57::E57Exception& e) + { + error = "E57 error while reading '" + path + "': " + std::string(e.what()) + " (" + e.context() + ")"; + scans.clear(); + return false; + } catch (const std::exception& e) + { + error = "error while reading '" + path + "': " + e.what(); + scans.clear(); + return false; + } + + if (scans.empty()) + { + if (error.empty()) + error = "no readable scans in '" + path + "'"; + return false; + } + return true; + } + + bool rewrite_e57_poses( + const std::string& src_path, const std::string& dst_path, const std::map& new_poses, std::string& error) + { + error.clear(); + if (src_path == dst_path) + { + error = "rewrite_e57_poses: source and destination path must differ"; + return false; + } + + try + { + ::e57::Reader reader(src_path, ::e57::ReaderOptions{}); + if (!reader.IsOpen()) + { + error = "could not open '" + src_path + "'"; + return false; + } + + ::e57::E57Root root; + reader.GetE57Root(root); + + ::e57::WriterOptions wopts; + wopts.coordinateMetadata = root.coordinateMetadata; // fresh file guid on purpose + ::e57::Writer writer(dst_path, wopts); + if (!writer.IsOpen()) + { + error = "could not create '" + dst_path + "'"; + return false; + } + + std::map pose_deltas; // Data3D guid -> new * old^-1 + + const int64_t scan_count = reader.GetData3DCount(); + for (int64_t i = 0; i < scan_count; i++) + { + ::e57::Data3D header; + if (!reader.ReadData3D(i, header)) + throw std::runtime_error("failed to read Data3D header " + std::to_string(i)); + + int64_t row_max = 0, col_max = 0, points_size = 0, groups_size = 0, count_size = 0; + bool column_index = false; + reader.GetData3DSizes(i, row_max, col_max, points_size, groups_size, count_size, column_index); + if (points_size <= 0) + points_size = static_cast(header.pointCount); + const size_t total = points_size > 0 ? static_cast(points_size) : 0; + + const auto it = new_poses.find(static_cast(i)); + if (it != new_poses.end()) + { + const Eigen::Affine3d old_pose = pose_from_header(header); + header.pose = rbt_from_affine(it->second); + if (!header.guid.empty()) + pose_deltas[header.guid] = it->second * old_pose.inverse(); + spdlog::info("e57: updating pose of Data3D {} ('{}') in '{}'", i, header.name, src_path); + } + + header.pointCount = total; + const int64_t di = writer.NewData3D(header); + + if (total > 0) + { + const size_t chunk = std::min(kChunk, total); + + ::e57::Data3DPointsDouble buffers; + CopyBuffers storage; + storage.bind(header, chunk, buffers); + + ::e57::CompressedVectorReader cvr = reader.SetUpData3DPointsData(i, chunk, buffers); + ::e57::CompressedVectorWriter cvw = writer.SetUpData3DPointsData(di, chunk, buffers); + unsigned got = 0; + while ((got = cvr.read()) > 0) + cvw.write(got); + cvw.close(); + cvr.close(); + } + + if (groups_size > 0) + { + std::vector id_elem(static_cast(groups_size)); + std::vector start_idx(static_cast(groups_size)); + std::vector point_count(static_cast(groups_size)); + if (reader.ReadData3DGroupsData( + i, static_cast(groups_size), id_elem.data(), start_idx.data(), point_count.data())) + writer.WriteData3DGroupsData( + di, static_cast(groups_size), id_elem.data(), start_idx.data(), point_count.data()); + } + } + + const int64_t image_count = reader.GetImage2DCount(); + for (int64_t k = 0; k < image_count; k++) + copy_image2d(reader, writer, k, pose_deltas); + + if (!writer.Close()) + { + error = "failed to finalize '" + dst_path + "'"; + return false; + } + } catch (const ::e57::E57Exception& e) + { + error = "E57 error while rewriting '" + src_path + "': " + std::string(e.what()) + " (" + e.context() + ")"; + return false; + } catch (const std::exception& e) + { + error = "error while rewriting '" + src_path + "': " + e.what(); + return false; + } + + return true; + } + + bool save_e57(const std::string& dst_path, const std::vector& scans, std::string& error) + { + error.clear(); + + std::size_t total_written = 0; + try + { + ::e57::Writer writer(dst_path, ::e57::WriterOptions{}); + if (!writer.IsOpen()) + { + error = "could not create '" + dst_path + "'"; + return false; + } + + for (std::size_t si = 0; si < scans.size(); si++) + { + const auto& s = scans[si]; + if (s.points == nullptr || s.points->empty()) + { + spdlog::warn("save_e57: scan {} ('{}') has no points, skipping", si, s.name); + continue; + } + + const std::size_t n = s.points->size(); + const bool has_i = s.intensities != nullptr && s.intensities->size() == n; + const bool has_c = s.colors != nullptr && s.colors->size() == n; + const bool has_t = s.timestamps != nullptr && s.timestamps->size() == n; + + ::e57::Data3D header; + header.name = s.name.empty() ? ("scan_" + std::to_string(si)) : s.name; + header.description = s.description; + header.pointCount = n; + header.pose = rbt_from_affine(s.pose); + + auto& pf = header.pointFields; + pf.cartesianXField = true; + pf.cartesianYField = true; + pf.cartesianZField = true; + pf.pointRangeNodeType = ::e57::NumericalNodeType::Double; + if (has_i) + { + pf.intensityField = true; + pf.intensityNodeType = ::e57::NumericalNodeType::Float; + header.intensityLimits = { 0.0, 65535.0 }; + } + if (has_c) + { + pf.colorRedField = true; + pf.colorGreenField = true; + pf.colorBlueField = true; + header.colorLimits = { 0.0, 255.0, 0.0, 255.0, 0.0, 255.0 }; + } + if (has_t) + { + pf.timeStampField = true; + pf.timeNodeType = ::e57::NumericalNodeType::Double; + } + + Eigen::Vector3d lo = (*s.points)[0]; + Eigen::Vector3d hi = (*s.points)[0]; + for (const auto& p : *s.points) + { + lo = lo.cwiseMin(p); + hi = hi.cwiseMax(p); + } + header.cartesianBounds.xMinimum = lo.x(); + header.cartesianBounds.yMinimum = lo.y(); + header.cartesianBounds.zMinimum = lo.z(); + header.cartesianBounds.xMaximum = hi.x(); + header.cartesianBounds.yMaximum = hi.y(); + header.cartesianBounds.zMaximum = hi.z(); + + const int64_t di = writer.NewData3D(header); + + const std::size_t chunk = std::min(kChunk, n); + std::vector cx(chunk), cy(chunk), cz(chunk), inten, tim; + std::vector cr, cg, cb; + + ::e57::Data3DPointsDouble buffers; + buffers.cartesianX = cx.data(); + buffers.cartesianY = cy.data(); + buffers.cartesianZ = cz.data(); + if (has_i) + { + inten.resize(chunk); + buffers.intensity = inten.data(); + } + if (has_c) + { + cr.resize(chunk); + cg.resize(chunk); + cb.resize(chunk); + buffers.colorRed = cr.data(); + buffers.colorGreen = cg.data(); + buffers.colorBlue = cb.data(); + } + if (has_t) + { + tim.resize(chunk); + buffers.timeStamp = tim.data(); + } + + ::e57::CompressedVectorWriter vw = writer.SetUpData3DPointsData(di, chunk, buffers); + std::size_t done = 0; + while (done < n) + { + const std::size_t m = std::min(chunk, n - done); + for (std::size_t k = 0; k < m; k++) + { + const Eigen::Vector3d& p = (*s.points)[done + k]; + cx[k] = p.x(); + cy[k] = p.y(); + cz[k] = p.z(); + if (has_i) + inten[k] = static_cast((*s.intensities)[done + k]); + if (has_c) + { + const Eigen::Vector3d& col = (*s.colors)[done + k]; + cr[k] = static_cast(std::lround(std::clamp(col.x(), 0.0, 1.0) * 255.0)); + cg[k] = static_cast(std::lround(std::clamp(col.y(), 0.0, 1.0) * 255.0)); + cb[k] = static_cast(std::lround(std::clamp(col.z(), 0.0, 1.0) * 255.0)); + } + if (has_t) + tim[k] = (*s.timestamps)[done + k]; + } + vw.write(m); + done += m; + } + vw.close(); + + total_written++; + spdlog::info( + "save_e57: wrote scan '{}' ({} points{}{}{}) to '{}'", + header.name, + n, + has_i ? ", intensity" : "", + has_c ? ", rgb" : "", + has_t ? ", time" : "", + dst_path); + } + + if (!writer.Close()) + { + error = "failed to finalize '" + dst_path + "'"; + return false; + } + } catch (const ::e57::E57Exception& e) + { + error = "E57 error while writing '" + dst_path + "': " + std::string(e.what()) + " (" + e.context() + ")"; + return false; + } catch (const std::exception& e) + { + error = "error while writing '" + dst_path + "': " + e.what(); + return false; + } + + if (total_written == 0) + { + error = "no non-empty scans to write"; + return false; + } + return true; + } +} // namespace mandeye::e57io