From 98b64020e41a9aa3c847caa96af66f987a2f4887 Mon Sep 17 00:00:00 2001 From: Artur Mukhamadiev Date: Thu, 27 Aug 2026 15:19:18 +0300 Subject: [PATCH] feat(cloud_point): benchmark SCARED reconstruction accuracy - Load pixel-aligned SCARED OBJ ground truth with unit conversion - Rectify left-camera XYZ maps into the reconstruction coordinate frame - Compute coverage, component errors, 3-D errors, and accuracy thresholds - Add a JSON benchmark executable and focused evaluator/loader tests - Document benchmark usage and disparity configuration TG-2 #ready-for-test --- README.md | 14 +- include/cloud_point/point_cloud_evaluator.hpp | 50 +++++++ .../scared_ground_truth_loader.hpp | 40 ++++++ include/cloud_point/stereo_rectifier.hpp | 16 ++- src/cloud_point/meson.build | 9 ++ src/cloud_point/point_cloud_evaluator.cpp | 124 ++++++++++++++++++ src/cloud_point/scared_dataset_benchmark.cpp | 121 +++++++++++++++++ .../scared_ground_truth_loader.cpp | 112 ++++++++++++++++ src/cloud_point/stereo_rectifier.cpp | 53 +++++++- tests/meson.build | 4 +- tests/test_point_cloud_evaluator.cpp | 101 ++++++++++++++ tests/test_scared_ground_truth_loader.cpp | 79 +++++++++++ tests/test_stereo_rectifier.cpp | 25 ++++ 13 files changed, 740 insertions(+), 8 deletions(-) create mode 100644 include/cloud_point/point_cloud_evaluator.hpp create mode 100644 include/cloud_point/scared_ground_truth_loader.hpp create mode 100644 src/cloud_point/point_cloud_evaluator.cpp create mode 100644 src/cloud_point/scared_dataset_benchmark.cpp create mode 100644 src/cloud_point/scared_ground_truth_loader.cpp create mode 100644 tests/test_point_cloud_evaluator.cpp create mode 100644 tests/test_scared_ground_truth_loader.cpp diff --git a/README.md b/README.md index 99ab8ae..8c691f0 100644 --- a/README.md +++ b/README.md @@ -162,8 +162,18 @@ In a second terminal run the interactive CLI against the same host and port: # Option 5 — compute point cloud and save to output.ply (inspect in MeshLab) ``` -The SCARED test set contains no ground-truth depth, so validation is -qualitative (inspect the PLY in MeshLab or similar). +When the keyframe contains `point_cloud.obj`, run the benchmark to compare the +reconstruction with its pixel-aligned XYZ ground truth: + +```bash +./build/src/cloud_point/scared_dataset_benchmark \ + /path/to/test_dataset_8/keyframe_0 160 +``` + +The benchmark reports coverage, component-wise and 3-D errors, threshold +accuracy, and matching/reconstruction timings as JSON. OBJ coordinates are +converted from millimetres to metres and rectified into the same left-camera +frame as the reconstructed cloud before evaluation. **Disparity range caveat:** the CLI constructs `CloudPointClient` with the default of 128 disparity levels, while this rig (fx ≈ 1024 px, baseline diff --git a/include/cloud_point/point_cloud_evaluator.hpp b/include/cloud_point/point_cloud_evaluator.hpp new file mode 100644 index 0000000..4b1bd51 --- /dev/null +++ b/include/cloud_point/point_cloud_evaluator.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include + +#include "cloud_point/point_cloud_builder.hpp" + +namespace score { + +/// @brief Accuracy and completeness measurements for an XYZ reconstruction. +struct PointCloudMetrics { + std::size_t ground_truth_points{0}; + std::size_t matched_points{0}; + + /// Fraction of valid ground-truth pixels with a finite prediction. + double coverage{0.0}; + + /// Absolute component errors over matched points, in metres. + double mae_x_m{std::numeric_limits::quiet_NaN()}; + double mae_y_m{std::numeric_limits::quiet_NaN()}; + double mae_z_m{std::numeric_limits::quiet_NaN()}; + + /// Euclidean XYZ errors over matched points, in metres. These are NaN when + /// no ground-truth pixel has a valid prediction. + double mae_3d_m{std::numeric_limits::quiet_NaN()}; + double rmse_3d_m{std::numeric_limits::quiet_NaN()}; + double median_3d_m{std::numeric_limits::quiet_NaN()}; + + /// Fractions of all valid ground-truth pixels reconstructed within the + /// threshold. Missing predictions therefore count as failures. + double within_1mm{0.0}; + double within_2mm{0.0}; + double within_5mm{0.0}; +}; + +/// @brief Pixel-aligned evaluation of a reconstructed point cloud. +class PointCloudEvaluator { + public: + /// @param predicted Dense point cloud in the rectified-left frame, metres. + /// @param ground_truth CV_32FC3 point map in the same frame and dimensions, + /// metres. Any point with a non-finite component is unknown. + /// @throws std::invalid_argument on a type, size, or storage mismatch, or + /// when the ground-truth map has no valid points. + [[nodiscard]] static PointCloudMetrics + evaluate(const PointCloud &predicted, const cv::Mat &ground_truth); +}; + +} // namespace score diff --git a/include/cloud_point/scared_ground_truth_loader.hpp b/include/cloud_point/scared_ground_truth_loader.hpp new file mode 100644 index 0000000..86beefa --- /dev/null +++ b/include/cloud_point/scared_ground_truth_loader.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include + +#include + +namespace score { + +/// @brief Loads the semi-dense XYZ point map supplied with a SCARED keyframe. +/// +/// The OBJ vertex order is preserved: vertex r * width + c belongs to pixel +/// (r, c). Unknown vertices are represented as NaN in all three coordinates. +/// The returned points are in the original left-camera coordinate frame. +class ScaredGroundTruthLoader { + public: + /// @brief Load /point_cloud.obj. + /// @param keyframe_dir Directory containing the SCARED keyframe files. + /// @param image_size Expected point-map dimensions. + /// @param units_to_metres Scale applied to finite OBJ coordinates. SCARED + /// ground truth is normally expressed in millimetres. + /// @throws std::invalid_argument for invalid dimensions or scale. + /// @throws std::runtime_error if the OBJ cannot be read or does not contain + /// exactly image_size.area() vertex records. + explicit ScaredGroundTruthLoader(const std::string &keyframe_dir, + cv::Size image_size, + float units_to_metres = 0.001f); + + /// @brief Ground-truth XYZ point map, CV_32FC3 in metres. + [[nodiscard]] const cv::Mat &point_map() const noexcept; + + /// @brief Number of pixels with finite XYZ ground truth. + [[nodiscard]] std::size_t valid_point_count() const noexcept; + + private: + cv::Mat point_map_; + std::size_t valid_point_count_{0}; +}; + +} // namespace score diff --git a/include/cloud_point/stereo_rectifier.hpp b/include/cloud_point/stereo_rectifier.hpp index 828ac80..b8bec8b 100644 --- a/include/cloud_point/stereo_rectifier.hpp +++ b/include/cloud_point/stereo_rectifier.hpp @@ -11,8 +11,8 @@ namespace score { /// @brief Stereo rectifier that computes rectification maps from calibration /// data and applies them to image pairs. /// -/// Thread safety: const methods (rectify, q) are safe to call concurrently. -/// The object must not be modified after construction. +/// Thread safety: const methods (rectify, rectify_left_point_map, q) are safe +/// to call concurrently. The object must not be modified after construction. class StereoRectifier { public: /// @brief Calibration parameters for a stereo rig. @@ -48,12 +48,24 @@ class StereoRectifier { [[nodiscard]] std::pair rectify(const cv::Mat &left, const cv::Mat &right) const; + /// @brief Rectify an XYZ point map from the original left-camera frame. + /// + /// The map is resampled with nearest-neighbour interpolation to avoid + /// blending geometry or NaN values, then finite points are rotated into + /// the rectified-left coordinate frame. + /// @param point_map CV_32FC3 map with the calibration image dimensions. + /// @return CV_32FC3 point map in the rectified-left frame. + /// @throws std::invalid_argument for an invalid type or dimensions. + [[nodiscard]] cv::Mat + rectify_left_point_map(const cv::Mat &point_map) const; + /// @brief Access the 4x4 reprojection matrix Q produced by stereoRectify. [[nodiscard]] const cv::Mat &q() const noexcept; private: cv::Mat map_lx_, map_ly_; ///< Rectification maps for the left image cv::Mat map_rx_, map_ry_; ///< Rectification maps for the right image + cv::Mat r1_; ///< Rotation into the rectified-left frame cv::Mat q_; ///< 4x4 CV_64F reprojection matrix }; diff --git a/src/cloud_point/meson.build b/src/cloud_point/meson.build index 6101343..df663ab 100644 --- a/src/cloud_point/meson.build +++ b/src/cloud_point/meson.build @@ -18,8 +18,10 @@ cloud_point_sources = files( 'stereo_matcher_factory.cpp', 'stereo_rectifier.cpp', 'point_cloud_builder.cpp', + 'point_cloud_evaluator.cpp', 'cloud_point_client.cpp', 'scared_dataset_loader.cpp', + 'scared_ground_truth_loader.cpp', ) cpc_deps = [ cloud_point_rpc_dep, opencv_dep ] @@ -48,3 +50,10 @@ executable( dependencies: [cloud_point_compute_dep], install: true, ) + +executable( + 'scared_dataset_benchmark', + 'scared_dataset_benchmark.cpp', + dependencies: [cloud_point_compute_dep], + install: true, +) diff --git a/src/cloud_point/point_cloud_evaluator.cpp b/src/cloud_point/point_cloud_evaluator.cpp new file mode 100644 index 0000000..7d9a27d --- /dev/null +++ b/src/cloud_point/point_cloud_evaluator.cpp @@ -0,0 +1,124 @@ +#include "cloud_point/point_cloud_evaluator.hpp" + +#include +#include +#include +#include + +namespace score { + +namespace { + +bool finite(const cv::Vec3f &point) { + return std::isfinite(point[0]) && std::isfinite(point[1]) && + std::isfinite(point[2]); +} + +} // namespace + +PointCloudMetrics PointCloudEvaluator::evaluate(const PointCloud &predicted, + const cv::Mat &ground_truth) { + if (ground_truth.type() != CV_32FC3) { + throw std::invalid_argument( + "PointCloudEvaluator: ground truth must be CV_32FC3"); + } + if (predicted.width != ground_truth.cols || + predicted.height != ground_truth.rows) { + throw std::invalid_argument( + "PointCloudEvaluator: prediction and ground-truth dimensions do " + "not match"); + } + + const auto pixel_count = static_cast(predicted.width) * + static_cast(predicted.height); + if (predicted.data.size() != pixel_count * 3u) { + throw std::invalid_argument( + "PointCloudEvaluator: prediction storage size is invalid"); + } + + PointCloudMetrics metrics; + std::vector errors; + errors.reserve(pixel_count); + double sum_abs_x = 0.0; + double sum_abs_y = 0.0; + double sum_abs_z = 0.0; + double sum_error = 0.0; + double sum_error_squared = 0.0; + std::size_t within_1mm = 0; + std::size_t within_2mm = 0; + std::size_t within_5mm = 0; + + for (int row = 0; row < ground_truth.rows; ++row) { + for (int column = 0; column < ground_truth.cols; ++column) { + const cv::Vec3f truth = ground_truth.at(row, column); + if (!finite(truth)) { + continue; + } + ++metrics.ground_truth_points; + + const auto index = + (static_cast(row) * predicted.width + column) * 3u; + const cv::Vec3f estimate(predicted.data[index], + predicted.data[index + 1], + predicted.data[index + 2]); + if (!finite(estimate)) { + continue; + } + + ++metrics.matched_points; + const cv::Vec3f delta = estimate - truth; + const double abs_x = std::abs(static_cast(delta[0])); + const double abs_y = std::abs(static_cast(delta[1])); + const double abs_z = std::abs(static_cast(delta[2])); + const double error = + std::sqrt(abs_x * abs_x + abs_y * abs_y + abs_z * abs_z); + + sum_abs_x += abs_x; + sum_abs_y += abs_y; + sum_abs_z += abs_z; + sum_error += error; + sum_error_squared += error * error; + errors.push_back(error); + within_1mm += error <= 0.001; + within_2mm += error <= 0.002; + within_5mm += error <= 0.005; + } + } + + if (metrics.ground_truth_points == 0) { + throw std::invalid_argument( + "PointCloudEvaluator: ground truth has no valid points"); + } + + const double ground_truth_count = + static_cast(metrics.ground_truth_points); + metrics.coverage = + static_cast(metrics.matched_points) / ground_truth_count; + metrics.within_1mm = static_cast(within_1mm) / ground_truth_count; + metrics.within_2mm = static_cast(within_2mm) / ground_truth_count; + metrics.within_5mm = static_cast(within_5mm) / ground_truth_count; + + if (metrics.matched_points == 0) { + return metrics; + } + + const double matched_count = static_cast(metrics.matched_points); + metrics.mae_x_m = sum_abs_x / matched_count; + metrics.mae_y_m = sum_abs_y / matched_count; + metrics.mae_z_m = sum_abs_z / matched_count; + metrics.mae_3d_m = sum_error / matched_count; + metrics.rmse_3d_m = std::sqrt(sum_error_squared / matched_count); + + const auto middle = + errors.begin() + static_cast(errors.size() / 2); + std::nth_element(errors.begin(), middle, errors.end()); + metrics.median_3d_m = *middle; + if (errors.size() % 2 == 0) { + const auto lower = std::max_element(errors.begin(), middle); + metrics.median_3d_m = (*lower + *middle) / 2.0; + } + + return metrics; +} + +} // namespace score diff --git a/src/cloud_point/scared_dataset_benchmark.cpp b/src/cloud_point/scared_dataset_benchmark.cpp new file mode 100644 index 0000000..5c2d3ba --- /dev/null +++ b/src/cloud_point/scared_dataset_benchmark.cpp @@ -0,0 +1,121 @@ +/// @file scared_dataset_benchmark.cpp +/// @brief Evaluate the CPU stereo reconstruction against SCARED XYZ ground +/// truth. +#include +#include +#include +#include + +#include +#include +#include + +#include "cloud_point/imageFactory.h" +#include "cloud_point/point_cloud_builder.hpp" +#include "cloud_point/point_cloud_evaluator.hpp" +#include "cloud_point/scared_dataset_loader.hpp" +#include "cloud_point/scared_ground_truth_loader.hpp" +#include "cloud_point/stereo_matcher_factory.hpp" +#include "cloud_point/stereo_rectifier.hpp" + +namespace { + +cv::Mat to_gray(const score::ImageRPC &rpc) { + auto image = score::ImageFactory::create(rpc); + cv::Mat gray; + switch (rpc.type) { + case score::ImageRPC::Type::BGR: + cv::cvtColor(image.get(), gray, cv::COLOR_BGR2GRAY); + break; + case score::ImageRPC::Type::RGBA: + cv::cvtColor(image.get(), gray, cv::COLOR_RGBA2GRAY); + break; + default: + throw std::invalid_argument( + "scared_dataset_benchmark: expected a colour stereo image"); + } + return gray; +} + +} // namespace + +int main(int argc, char *argv[]) { + google::InitGoogleLogging(argv[0]); + FLAGS_alsologtostderr = true; + + if (argc < 2 || argc > 3) { + std::cerr << "Usage: " << argv[0] + << " [num_disparities]\n"; + return 1; + } + + try { + const std::string keyframe_dir = argv[1]; + const int num_disparities = argc == 3 ? std::stoi(argv[2]) : 160; + + score::ScaredDatasetLoader dataset(keyframe_dir); + const auto &calibration_rpc = dataset.calibration(); + score::ScaredGroundTruthLoader ground_truth( + keyframe_dir, + cv::Size(calibration_rpc.width, calibration_rpc.height)); + + const auto calibration = + score::StereoRectifier::Calibration::from_rpc(calibration_rpc); + score::StereoRectifier rectifier(calibration); + auto matcher = score::StereoMatcherFactory::create( + score::StereoAlgorithmType::CPU, num_disparities); + score::PointCloudBuilder builder(rectifier.q()); + + const auto pair = dataset.image_pair(0); + const cv::Mat left_gray = to_gray(pair.left); + const cv::Mat right_gray = to_gray(pair.right); + auto [rectified_left, rectified_right] = + rectifier.rectify(left_gray, right_gray); + + const auto start = std::chrono::steady_clock::now(); + const cv::Mat disparity = + matcher->compute(rectified_left, rectified_right); + const auto matching_end = std::chrono::steady_clock::now(); + const score::PointCloud cloud = builder.build(disparity); + const auto reconstruction_end = std::chrono::steady_clock::now(); + + const cv::Mat rectified_ground_truth = + rectifier.rectify_left_point_map(ground_truth.point_map()); + const auto metrics = + score::PointCloudEvaluator::evaluate(cloud, rectified_ground_truth); + + const auto matching_ms = + std::chrono::duration(matching_end - start) + .count(); + const auto reconstruction_ms = + std::chrono::duration(reconstruction_end - + matching_end) + .count(); + + const nlohmann::json output = { + {"keyframe_dir", keyframe_dir}, + {"algorithm", "StereoSGBM"}, + {"num_disparities", num_disparities}, + {"ground_truth_points", metrics.ground_truth_points}, + {"matched_points", metrics.matched_points}, + {"coverage", metrics.coverage}, + {"mae_x_m", metrics.mae_x_m}, + {"mae_y_m", metrics.mae_y_m}, + {"mae_z_m", metrics.mae_z_m}, + {"mae_3d_m", metrics.mae_3d_m}, + {"rmse_3d_m", metrics.rmse_3d_m}, + {"median_3d_m", metrics.median_3d_m}, + {"within_1mm", metrics.within_1mm}, + {"within_2mm", metrics.within_2mm}, + {"within_5mm", metrics.within_5mm}, + {"matching_ms", matching_ms}, + {"reconstruction_ms", reconstruction_ms}, + }; + std::cout << output.dump(2) << '\n'; + } catch (const std::exception &error) { + std::cerr << "Benchmark failed: " << error.what() << '\n'; + return 1; + } + + return 0; +} diff --git a/src/cloud_point/scared_ground_truth_loader.cpp b/src/cloud_point/scared_ground_truth_loader.cpp new file mode 100644 index 0000000..4a35de0 --- /dev/null +++ b/src/cloud_point/scared_ground_truth_loader.cpp @@ -0,0 +1,112 @@ +#include "cloud_point/scared_ground_truth_loader.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace score { + +namespace { + +float parse_coordinate(const std::string &token, const std::string &path, + std::size_t line_number) { + try { + std::size_t parsed = 0; + const float value = std::stof(token, &parsed); + if (parsed != token.size()) { + throw std::invalid_argument("trailing characters"); + } + return value; + } catch (const std::exception &) { + throw std::runtime_error( + "ScaredGroundTruthLoader: invalid coordinate at " + path + ":" + + std::to_string(line_number) + ": " + token); + } +} + +} // namespace + +ScaredGroundTruthLoader::ScaredGroundTruthLoader( + const std::string &keyframe_dir, cv::Size image_size, + float units_to_metres) { + if (image_size.width <= 0 || image_size.height <= 0) { + throw std::invalid_argument( + "ScaredGroundTruthLoader: image dimensions must be positive"); + } + if (!std::isfinite(units_to_metres) || units_to_metres <= 0.0f) { + throw std::invalid_argument( + "ScaredGroundTruthLoader: units_to_metres must be finite and " + "positive"); + } + + const std::string path = keyframe_dir + "/point_cloud.obj"; + std::ifstream input(path); + if (!input) { + throw std::runtime_error( + "ScaredGroundTruthLoader: cannot open point cloud: " + path); + } + + const auto expected_count = static_cast(image_size.width) * + static_cast(image_size.height); + std::vector vertices; + vertices.reserve(expected_count); + + std::string line; + std::size_t line_number = 0; + const float nan = std::numeric_limits::quiet_NaN(); + while (std::getline(input, line)) { + ++line_number; + std::istringstream stream(line); + std::string record; + stream >> record; + if (record != "v") { + continue; + } + + std::string x_token, y_token, z_token; + if (!(stream >> x_token >> y_token >> z_token)) { + throw std::runtime_error( + "ScaredGroundTruthLoader: incomplete vertex at " + path + ":" + + std::to_string(line_number)); + } + + const float x = parse_coordinate(x_token, path, line_number); + const float y = parse_coordinate(y_token, path, line_number); + const float z = parse_coordinate(z_token, path, line_number); + if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { + vertices.emplace_back(x * units_to_metres, y * units_to_metres, + z * units_to_metres); + ++valid_point_count_; + } else { + vertices.emplace_back(nan, nan, nan); + } + } + + if (vertices.size() != expected_count) { + throw std::runtime_error("ScaredGroundTruthLoader: expected " + + std::to_string(expected_count) + + " vertices in " + path + ", got " + + std::to_string(vertices.size())); + } + + point_map_ = cv::Mat(image_size, CV_32FC3); + for (std::size_t index = 0; index < vertices.size(); ++index) { + point_map_.at(static_cast(index / image_size.width), + static_cast(index % image_size.width)) = + vertices[index]; + } +} + +const cv::Mat &ScaredGroundTruthLoader::point_map() const noexcept { + return point_map_; +} + +std::size_t ScaredGroundTruthLoader::valid_point_count() const noexcept { + return valid_point_count_; +} + +} // namespace score diff --git a/src/cloud_point/stereo_rectifier.cpp b/src/cloud_point/stereo_rectifier.cpp index 38472cc..812094d 100644 --- a/src/cloud_point/stereo_rectifier.cpp +++ b/src/cloud_point/stereo_rectifier.cpp @@ -1,5 +1,7 @@ #include "cloud_point/stereo_rectifier.hpp" +#include +#include #include #include @@ -58,12 +60,12 @@ StereoRectifier::StereoRectifier(const Calibration &calib) { require_mat(calib.r, 3, 3, CV_64F, "r"); require_mat(calib.t, 3, 1, CV_64F, "t"); - cv::Mat R1, R2, P1, P2; + cv::Mat R2, P1, P2; cv::stereoRectify(calib.k_left, calib.d_left, calib.k_right, calib.d_right, - calib.image_size, calib.r, calib.t, R1, R2, P1, P2, q_, + calib.image_size, calib.r, calib.t, r1_, R2, P1, P2, q_, cv::CALIB_ZERO_DISPARITY, /*alpha=*/0); - cv::initUndistortRectifyMap(calib.k_left, calib.d_left, R1, P1, + cv::initUndistortRectifyMap(calib.k_left, calib.d_left, r1_, P1, calib.image_size, CV_16SC2, map_lx_, map_ly_); cv::initUndistortRectifyMap(calib.k_right, calib.d_right, R2, P2, calib.image_size, CV_16SC2, map_rx_, map_ry_); @@ -81,6 +83,51 @@ StereoRectifier::rectify(const cv::Mat &left, const cv::Mat &right) const { return {rect_left, rect_right}; } +// --------------------------------------------------------------------------- +// StereoRectifier::rectify_left_point_map +// --------------------------------------------------------------------------- + +cv::Mat +StereoRectifier::rectify_left_point_map(const cv::Mat &point_map) const { + if (point_map.type() != CV_32FC3) { + throw std::invalid_argument("point_map must be CV_32FC3, got type=" + + std::to_string(point_map.type())); + } + if (point_map.size() != map_lx_.size()) { + throw std::invalid_argument( + "point_map dimensions must match the calibration image size"); + } + + const float nan = std::numeric_limits::quiet_NaN(); + cv::Mat rectified; + cv::remap(point_map, rectified, map_lx_, map_ly_, cv::INTER_NEAREST, + cv::BORDER_CONSTANT, cv::Scalar(nan, nan, nan)); + + cv::Mat rotation; + r1_.convertTo(rotation, CV_32F); + for (int row = 0; row < rectified.rows; ++row) { + for (int column = 0; column < rectified.cols; ++column) { + auto &point = rectified.at(row, column); + if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || + !std::isfinite(point[2])) { + point = cv::Vec3f(nan, nan, nan); + continue; + } + + point = cv::Vec3f(rotation.at(0, 0) * point[0] + + rotation.at(0, 1) * point[1] + + rotation.at(0, 2) * point[2], + rotation.at(1, 0) * point[0] + + rotation.at(1, 1) * point[1] + + rotation.at(1, 2) * point[2], + rotation.at(2, 0) * point[0] + + rotation.at(2, 1) * point[1] + + rotation.at(2, 2) * point[2]); + } + } + return rectified; +} + // --------------------------------------------------------------------------- // StereoRectifier::q // --------------------------------------------------------------------------- diff --git a/tests/meson.build b/tests/meson.build index 2c63c04..13d95af 100644 --- a/tests/meson.build +++ b/tests/meson.build @@ -18,8 +18,10 @@ if opencv_dep.found() 'test_stereo_matcher.cpp', 'test_stereo_rectifier.cpp', 'test_point_cloud_builder.cpp', + 'test_point_cloud_evaluator.cpp', 'test_cloud_point_client.cpp', - 'test_scared_dataset.cpp' + 'test_scared_dataset.cpp', + 'test_scared_ground_truth_loader.cpp' ) test_deps += [cloud_point_compute_dep] else diff --git a/tests/test_point_cloud_evaluator.cpp b/tests/test_point_cloud_evaluator.cpp new file mode 100644 index 0000000..a32f174 --- /dev/null +++ b/tests/test_point_cloud_evaluator.cpp @@ -0,0 +1,101 @@ +#include + +#include +#include + +#include + +#include "cloud_point/point_cloud_evaluator.hpp" + +namespace { + +score::PointCloud make_cloud(const cv::Mat &points) { + score::PointCloud cloud; + cloud.width = points.cols; + cloud.height = points.rows; + cloud.data.reserve(static_cast(points.total()) * 3u); + for (int row = 0; row < points.rows; ++row) { + for (int column = 0; column < points.cols; ++column) { + const auto point = points.at(row, column); + cloud.data.push_back(point[0]); + cloud.data.push_back(point[1]); + cloud.data.push_back(point[2]); + } + } + return cloud; +} + +} // namespace + +TEST(PointCloudEvaluatorTest, ExactPredictionHasPerfectMetrics) { + const cv::Mat truth(1, 2, CV_32FC3, cv::Scalar(0.1f, 0.2f, 0.3f)); + const auto metrics = + score::PointCloudEvaluator::evaluate(make_cloud(truth), truth); + + EXPECT_EQ(metrics.ground_truth_points, 2u); + EXPECT_EQ(metrics.matched_points, 2u); + EXPECT_DOUBLE_EQ(metrics.coverage, 1.0); + EXPECT_DOUBLE_EQ(metrics.rmse_3d_m, 0.0); + EXPECT_DOUBLE_EQ(metrics.within_1mm, 1.0); + EXPECT_DOUBLE_EQ(metrics.within_2mm, 1.0); + EXPECT_DOUBLE_EQ(metrics.within_5mm, 1.0); +} + +TEST(PointCloudEvaluatorTest, MeasuresFullXyzEuclideanError) { + cv::Mat truth(1, 1, CV_32FC3, cv::Scalar(0.0f, 0.0f, 1.0f)); + cv::Mat prediction(1, 1, CV_32FC3, cv::Scalar(0.001f, 0.002f, 1.002f)); + + const auto metrics = + score::PointCloudEvaluator::evaluate(make_cloud(prediction), truth); + const double expected = 0.003; + + EXPECT_NEAR(metrics.mae_x_m, 0.001, 1e-7); + EXPECT_NEAR(metrics.mae_y_m, 0.002, 1e-7); + EXPECT_NEAR(metrics.mae_z_m, 0.002, 1e-7); + EXPECT_NEAR(metrics.mae_3d_m, expected, 1e-7); + EXPECT_NEAR(metrics.rmse_3d_m, expected, 1e-7); + EXPECT_DOUBLE_EQ(metrics.within_2mm, 0.0); + EXPECT_DOUBLE_EQ(metrics.within_5mm, 1.0); +} + +TEST(PointCloudEvaluatorTest, MissingPredictionReducesCoverageAndAccuracy) { + cv::Mat truth(1, 2, CV_32FC3, cv::Scalar(0.0f, 0.0f, 1.0f)); + cv::Mat prediction = truth.clone(); + const float nan = std::numeric_limits::quiet_NaN(); + prediction.at(0, 1) = cv::Vec3f(nan, nan, nan); + + const auto metrics = + score::PointCloudEvaluator::evaluate(make_cloud(prediction), truth); + + EXPECT_EQ(metrics.ground_truth_points, 2u); + EXPECT_EQ(metrics.matched_points, 1u); + EXPECT_DOUBLE_EQ(metrics.coverage, 0.5); + EXPECT_DOUBLE_EQ(metrics.within_1mm, 0.5); +} + +TEST(PointCloudEvaluatorTest, NoPredictionsReportsNaNErrors) { + cv::Mat truth(1, 1, CV_32FC3, cv::Scalar(0.0f, 0.0f, 1.0f)); + const float nan = std::numeric_limits::quiet_NaN(); + cv::Mat prediction(1, 1, CV_32FC3, cv::Scalar(nan, nan, nan)); + + const auto metrics = + score::PointCloudEvaluator::evaluate(make_cloud(prediction), truth); + + EXPECT_DOUBLE_EQ(metrics.coverage, 0.0); + EXPECT_TRUE(std::isnan(metrics.mae_3d_m)); + EXPECT_TRUE(std::isnan(metrics.rmse_3d_m)); +} + +TEST(PointCloudEvaluatorTest, IgnoresUnknownGroundTruthPixels) { + const float nan = std::numeric_limits::quiet_NaN(); + cv::Mat truth(1, 2, CV_32FC3); + truth.at(0, 0) = cv::Vec3f(0.0f, 0.0f, 1.0f); + truth.at(0, 1) = cv::Vec3f(nan, nan, nan); + cv::Mat prediction(1, 2, CV_32FC3, cv::Scalar(0.0f, 0.0f, 1.0f)); + + const auto metrics = + score::PointCloudEvaluator::evaluate(make_cloud(prediction), truth); + EXPECT_EQ(metrics.ground_truth_points, 1u); + EXPECT_EQ(metrics.matched_points, 1u); + EXPECT_DOUBLE_EQ(metrics.coverage, 1.0); +} diff --git a/tests/test_scared_ground_truth_loader.cpp b/tests/test_scared_ground_truth_loader.cpp new file mode 100644 index 0000000..aeb8084 --- /dev/null +++ b/tests/test_scared_ground_truth_loader.cpp @@ -0,0 +1,79 @@ +#include + +#include +#include +#include +#include +#include + +#include "cloud_point/scared_ground_truth_loader.hpp" + +namespace { + +class ScaredGroundTruthLoaderTest : public ::testing::Test { + protected: + void SetUp() override { + const auto suffix = + std::chrono::steady_clock::now().time_since_epoch().count(); + directory_ = std::filesystem::temp_directory_path() / + ("scared-ground-truth-" + std::to_string(suffix)); + std::filesystem::create_directories(directory_); + } + + void TearDown() override { std::filesystem::remove_all(directory_); } + + void write_obj(const std::string &contents) const { + std::ofstream output(directory_ / "point_cloud.obj"); + output << contents; + } + + std::filesystem::path directory_; +}; + +} // namespace + +TEST_F(ScaredGroundTruthLoaderTest, PreservesOrderNaNsAndConvertsToMetres) { + write_obj("# two by two point map\n" + "v 1000 2000 3000\n" + "v nan nan nan\n" + "v -500 0 250\n" + "v 1 2 3\n" + "f 1 3 4\n"); + + const score::ScaredGroundTruthLoader loader(directory_.string(), + cv::Size(2, 2)); + const cv::Mat &points = loader.point_map(); + + EXPECT_EQ(points.type(), CV_32FC3); + EXPECT_EQ(points.size(), cv::Size(2, 2)); + EXPECT_EQ(loader.valid_point_count(), 3u); + + const cv::Vec3f first = points.at(0, 0); + EXPECT_FLOAT_EQ(first[0], 1.0f); + EXPECT_FLOAT_EQ(first[1], 2.0f); + EXPECT_FLOAT_EQ(first[2], 3.0f); + + const cv::Vec3f missing = points.at(0, 1); + EXPECT_TRUE(std::isnan(missing[0])); + EXPECT_TRUE(std::isnan(missing[1])); + EXPECT_TRUE(std::isnan(missing[2])); + + const cv::Vec3f third = points.at(1, 0); + EXPECT_FLOAT_EQ(third[0], -0.5f); + EXPECT_FLOAT_EQ(third[1], 0.0f); + EXPECT_FLOAT_EQ(third[2], 0.25f); +} + +TEST_F(ScaredGroundTruthLoaderTest, RejectsIncorrectVertexCount) { + write_obj("v 1 2 3\n"); + EXPECT_THROW( + score::ScaredGroundTruthLoader(directory_.string(), cv::Size(2, 2)), + std::runtime_error); +} + +TEST_F(ScaredGroundTruthLoaderTest, RejectsInvalidScale) { + write_obj("v 1 2 3\n"); + EXPECT_THROW(score::ScaredGroundTruthLoader(directory_.string(), + cv::Size(1, 1), 0.0f), + std::invalid_argument); +} diff --git a/tests/test_stereo_rectifier.cpp b/tests/test_stereo_rectifier.cpp index f658a29..3e04af7 100644 --- a/tests/test_stereo_rectifier.cpp +++ b/tests/test_stereo_rectifier.cpp @@ -207,6 +207,31 @@ TEST(StereoRectifierTest, RectifyNearIdentityWithZeroDistortion) { << " — expected near-identity for zero-distortion identical cameras"; } +// --------------------------------------------------------------------------- +// Ground-truth point-map rectification +// --------------------------------------------------------------------------- + +TEST(StereoRectifierTest, RectifyLeftPointMapPreservesIdentityGeometry) { + score::StereoRectifier rectifier(make_calib()); + cv::Mat points(kHeight, kWidth, CV_32FC3, cv::Scalar(0.1f, -0.2f, 1.5f)); + + const cv::Mat rectified = rectifier.rectify_left_point_map(points); + + EXPECT_EQ(rectified.type(), CV_32FC3); + EXPECT_EQ(rectified.size(), points.size()); + const cv::Vec3f center = rectified.at(kHeight / 2, kWidth / 2); + EXPECT_NEAR(center[0], 0.1f, 1e-6f); + EXPECT_NEAR(center[1], -0.2f, 1e-6f); + EXPECT_NEAR(center[2], 1.5f, 1e-6f); +} + +TEST(StereoRectifierTest, RectifyLeftPointMapRejectsWrongType) { + score::StereoRectifier rectifier(make_calib()); + cv::Mat points(kHeight, kWidth, CV_32FC1, cv::Scalar(1.0f)); + EXPECT_THROW(rectifier.rectify_left_point_map(points), + std::invalid_argument); +} + // --------------------------------------------------------------------------- // Invalid calibration → throws // ---------------------------------------------------------------------------