diff --git a/apps/icp-log-viewer/main.cpp b/apps/icp-log-viewer/main.cpp index a31ad7e0..fb0a8966 100644 --- a/apps/icp-log-viewer/main.cpp +++ b/apps/icp-log-viewer/main.cpp @@ -75,6 +75,12 @@ static TCLAP::ValueArg argAutoPlayPeriod( "mode.", false, 0.1, "period [seconds]", cmd); +static TCLAP::ValueArg argMinQuality( + "q", "min-quality", + "Minimum ICP quality (range [0,1], i.e. 0%-100%) to load a log file. " + "Files whose ICP result quality is below this threshold are skipped.", + false, 0.0, "quality [0,1]", cmd); + // =========== Declare global variables =========== #if MRPT_HAS_NANOGUI @@ -233,6 +239,48 @@ void main_show_gui() files[0].wholePath = argSingleFile.getValue(); } + // Apply minimum quality filter if requested: + if (argMinQuality.isSet()) + { + const double minQ = argMinQuality.getValue(); + if (minQ < 0.0 || minQ > 1.0) + { + THROW_EXCEPTION_FMT("--min-quality must be in [0,1]. Got: %.03f", minQ); + } + std::cout << "Applying minimum quality filter: q >= " << minQ << std::endl; + + mrpt::system::CDirectoryExplorer::TFileInfoList filteredFiles; + for (const auto& file : files) + { + mp2p_icp::LogRecord lr; + if (!lr.load_from_file(file.wholePath)) + { + std::cerr << " Warning: could not load '" << file.wholePath << "'" << std::endl; + continue; + } + if (lr.icpResult.quality >= minQ) + { + filteredFiles.push_back(file); + } + else + { + std::cout << " Skipping (quality=" << lr.icpResult.quality << " < " << minQ + << "): " << file.name << std::endl; + } + } + std::cout << "Quality filter: kept " << filteredFiles.size() << " / " << files.size() + << " files." << std::endl; + + if (files.empty()) + { + THROW_EXCEPTION_FMT( + "No log files passed --min-quality=%.03f. Lower the threshold or check input logs.", + minQ); + } + + files = std::move(filteredFiles); + } + // load files: for (const auto& file : files) { diff --git a/docs/source/app_icp-log-viewer.rst b/docs/source/app_icp-log-viewer.rst index 7b9aaea5..7724570c 100644 --- a/docs/source/app_icp-log-viewer.rst +++ b/docs/source/app_icp-log-viewer.rst @@ -4,7 +4,8 @@ Application: ``icp-log-viewer`` ===================================== -Debug ICP pipelines as never before! +``icp-log-viewer`` is an interactive GUI tool to inspect and debug ICP (Iterative Closest Point) +registration sessions stored as ``*.icplog`` files. .. raw:: html @@ -18,12 +19,25 @@ Debug ICP pipelines as never before! How to launch ------------------ -Once mp2p_icp is installed, move to the directory where your `*.icplog` files have been stored and run: +Once mp2p_icp is installed, move to the directory where your ``*.icplog`` files have been stored and run: .. code-block:: bash icp-log-viewer +You can also point it directly at a single file or a specific directory: + +.. code-block:: bash + + # Load all *.icplog files in a directory + icp-log-viewer -d /path/to/logs/ + + # Load a single file + icp-log-viewer -f /path/to/log.icplog + + # Only load files where the ICP quality is >= 0.5 (50%) + icp-log-viewer -d /path/to/logs/ --min-quality 0.5 + .. dropdown:: Complete command line argument help @@ -31,17 +45,22 @@ Once mp2p_icp is installed, move to the directory where your `*.icplog` files ha USAGE: - icp-log-viewer [--autoplay-period ] [-l ] - [-f ] [-d <.>] [-e ] [--] - [--version] [-h] + icp-log-viewer [--autoplay-period ] [-q ] + [-l ] [-f ] [-d <.>] [-e ] + [--] [--version] [-h] - Where: + Where: --autoplay-period The period (in seconds) between timestamps to load and show in autoplay mode. + -q , --min-quality + Minimum ICP quality (range [0,1], i.e. 0%-100%) to load a log file. + Files whose ICP result quality is below this threshold are skipped. + This is useful to focus inspection on well-converged ICP results. + -l , --load-plugins One or more (comma separated) *.so files to load as plugins @@ -52,7 +71,7 @@ Once mp2p_icp is installed, move to the directory where your `*.icplog` files ha Directory in which to search for *.icplog files. -e , --file-extension - Filename extension to look for. Default is `icplog` + Filename extension to look for. Default is ``icplog``. --, --ignore_rest Ignores the rest of the labeled arguments following this flag. @@ -67,4 +86,49 @@ Once mp2p_icp is installed, move to the directory where your `*.icplog` files ha GUI and feature explanation ------------------------------- -Write me! +The viewer window is divided into a left control panel and a main 3D viewport. + +**Log selector** + +A slider at the top of the panel lets you step through all loaded ``*.icplog`` entries +(filtered by ``--min-quality`` if requested at launch). Use the **Back** / **Forward** buttons +for single-step navigation, or enable **Autoplay** to advance automatically at the rate set by +``--autoplay-period``. + +**3D viewport** + +The main 3D view shows the *global* map and the *local* point cloud registered against it. +The initial-guess pose and the final ICP pose can be toggled independently. + +**ICP statistics panel** + +Numerical outputs shown for each selected log entry: + +- **Quality** — the ICP quality score in [0, 1] produced by the configured + ``QualityEvaluator`` (e.g. ``QualityEvaluator_PairedRatio``). +- **Log pose** — the final estimated SE(3) pose of the local map. +- **Initial guess** — the pose used to seed the ICP iteration. +- **Init→Final** — the correction applied by ICP on top of the initial guess. +- **Covariance / condition number** — uncertainty of the result. +- **Pairings** — number of point correspondences in the last ICP iteration. + +**Pairings visualisation** + +Enable **Show pairings** to draw the point correspondences found at the last ICP +iteration. Individual pairing types (point-to-point, point-to-plane, point-to-line, +covariance-to-covariance) can be toggled independently. + +**Filtering by quality at load time** + +When a directory contains many log files (e.g. from a full SLAM session) it is often +useful to inspect only the low-quality registrations that may have caused drift. Use +``--min-quality`` to restrict the loaded set: + +.. code-block:: bash + + # Inspect only high-quality (≥ 80 %) registrations + icp-log-viewer -d logs/ --min-quality 0.8 + + # Inspect only low-quality (< 20 %) registrations — invert by running with 0 + # and then sorting/filtering externally, or set a low threshold and browse manually + icp-log-viewer -d logs/ --min-quality 0.0