diff --git a/.gitignore b/.gitignore index 2184315..18decc9 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,6 @@ large_tool_results/ # IDE .idea/ + +# Point-cloud exports +*.ply diff --git a/README.md b/README.md index 8c691f0..736756b 100644 --- a/README.md +++ b/README.md @@ -151,17 +151,53 @@ Each keyframe directory contains: /path/to/test_dataset_8/keyframe_0 8080 ``` +If port 8080 is already taken on your machine (Docker's `rootlesskit` +commonly holds it) pass another port and update `server.port` in the CLI +config accordingly. Connecting the CLI to a foreign service on 8080 shows up +as `invalid JSON response from server` / `std::bad_alloc` errors. + ### Connecting with the CLI -In a second terminal run the interactive CLI against the same host and port: +In a second terminal run the interactive CLI with the SCARED-tuned config: ```bash -# Adjust ip/port in config.yaml if needed, then: -./build/src/cloud_point_rpc_cli config.yaml -# Option 4 — compute point cloud and print valid point count -# Option 5 — compute point cloud and save to output.ply (inspect in MeshLab) +./build/src/cloud_point_rpc_cli config.scared.yml +# Option 4 — compute point cloud and print valid point count + bounding box +# Option 5 — compute point cloud and save a triangulated PLY mesh ``` +`config.scared.yml` sets the optional `cloud_point` section that options 4/5 +honour: + +```yaml +cloud_point: + algorithm: cpu # "gpu" falls back to CPU when CUDA is unavailable + num_disparities: 160 # fx ~1024 px, baseline ~4.35 mm -> up to ~160 px + min_depth_m: 0.02 # endoscopic working range: 20 mm .. 300 mm + max_depth_m: 0.30 +``` + +Depth limits are a physical bound on the scene: with this rig depth is +roughly `4.45 m / disparity_px`, so any mismatch with a disparity below +~15 px reprojects metres away. Without the section the CLI falls back to the +generic defaults (GPU, 128 disparities, 0.01–10 m). The SGBM matcher itself +is configured with OpenCV's recommended smoothness penalties (P1 = 8·bs², +P2 = 32·bs²), a 5×5 block, uniqueness ratio 10, speckle filtering and a +left-right consistency check; see `CpuStereoMatcher::Params`. + +### Checking the result + +Option 4 should report a bounding box with z inside roughly +`[0.03, 0.16]` m for `test_dataset_8` keyframes. Option 5 writes a binary +little-endian PLY containing every valid point **and** a mesh triangulated +from the pixel grid (triangles are dropped where the longest edge exceeds 5 % of the local +depth, so the mesh breaks at occlusions). The mesh is what makes web viewers +usable: viewers such as Meshy's online PLY viewer fabricate a triangle from +every three consecutive vertices of a vertex-only PLY, which draws long +slivers across the surface and makes a correct cloud look like a fan of +rays. Pass `PlyOptions{false, 0.0f, false}` to `write_ply` for a points-only ASCII +file. + When the keyframe contains `point_cloud.obj`, run the benchmark to compare the reconstruction with its pixel-aligned XYZ ground truth: @@ -173,16 +209,14 @@ reconstruction with its pixel-aligned XYZ ground truth: 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. +frame as the reconstructed cloud before evaluation. Reference numbers for +`dataset_1/keyframe_1` with the tuned SGBM configuration: coverage ≈ 0.85, +MAE₃D ≈ 0.8 mm, RMSE₃D ≈ 1.4 mm, 80 % of points within 2 mm (the previous +unregularised SGBM gave MAE₃D ≈ 44 mm and RMSE₃D ≈ 289 mm). -**Disparity range caveat:** the CLI constructs `CloudPointClient` with the -default of 128 disparity levels, while this rig (fx ≈ 1024 px, baseline -≈ 4.35 mm) produces disparities above 128 px for tissue nearer than -~35 mm — those pixels silently drop out of the cloud. The E2E test -(`tests/test_scared_dataset.cpp`) passes `num_disparities = 160` for full -coverage; programmatic consumers should do the same via the -`CloudPointClient` constructor. For the CLI's qualitative check the default -is fine (observed median depth ≈ 115 mm is well within range). +The E2E test (`tests/test_scared_dataset.cpp`) exercises the same pipeline +with `num_disparities = 160` and asserts >50 000 valid points and a median +depth in `[0.02, 0.20]` m. ## Communication model diff --git a/config.scared.yml b/config.scared.yml new file mode 100644 index 0000000..d92cb30 --- /dev/null +++ b/config.scared.yml @@ -0,0 +1,11 @@ +# CLI configuration for validating against a SCARED keyframe served by +# scared_dataset_server (default port 8080). +server: + ip: "127.0.0.1" + port: 8080 + +cloud_point: + algorithm: cpu # "gpu" falls back to CPU when CUDA is unavailable + num_disparities: 160 # fx ~1024 px, baseline ~4.35 mm -> up to ~160 px + min_depth_m: 0.02 # endoscopic working range: 20 mm .. 300 mm + max_depth_m: 0.30 diff --git a/config.yml b/config.yml index fc904fb..ab22334 100644 --- a/config.yml +++ b/config.yml @@ -1,3 +1,11 @@ server: ip: "127.0.0.1" port: 9095 + +# Optional stereo reconstruction settings for CLI options 4/5. +# See config.scared.yml for values tuned to the SCARED endoscopic rig. +# cloud_point: +# algorithm: gpu # or cpu +# num_disparities: 128 # positive multiple of 16 +# min_depth_m: 0.01 +# max_depth_m: 10.0 diff --git a/include/cloud_point/cloud_point_client.hpp b/include/cloud_point/cloud_point_client.hpp index 08a9b86..2fbaf13 100644 --- a/include/cloud_point/cloud_point_client.hpp +++ b/include/cloud_point/cloud_point_client.hpp @@ -29,7 +29,8 @@ class CloudPointClient { /// @brief Construct client (does not connect). /// @param ip Server IP address. /// @param port Server port. - /// @param algo Stereo matching algorithm (GPU falls back to CPU if + /// @param algo Stereo matching algorithm (GPU falls back to CPU + /// if /// unavailable). /// @param opts Depth filtering options. /// @param num_disparities SGBM disparity levels (default 128; use 160 for @@ -68,9 +69,32 @@ class CloudPointClient { std::unique_ptr builder_; }; -/// @brief Write valid points as ASCII PLY (for MeshLab inspection). +/// @brief PLY export settings. +struct PlyOptions { + /// Emit a triangle mesh built from the organised pixel grid. Web viewers + /// (e.g. Meshy) fabricate triangles from consecutive vertices of a + /// vertex-only PLY, which draws long slivers across the surface; a real + /// mesh renders correctly everywhere and still contains every point. + bool triangulate; + /// Reject triangles whose longest edge exceeds this fraction of the + /// triangle's mean depth (breaks the mesh at occlusion boundaries). + float max_edge_depth_ratio; + /// Write binary_little_endian instead of ASCII. A full 1280x1024 mesh + /// is ~40 MB in binary versus ~85 MB in ASCII and parses much faster. + bool binary; + PlyOptions() noexcept + : triangulate(true), max_edge_depth_ratio(0.05f), binary(true) {} + PlyOptions(bool tri, float ratio, bool bin = true) noexcept + : triangulate(tri), max_edge_depth_ratio(ratio), binary(bin) {} +}; + +/// @brief Write valid points (and optionally a grid-triangulated mesh) as +/// PLY for MeshLab / web-viewer inspection. /// @param cloud Source point cloud. /// @param path Output file path. -void write_ply(const PointCloud &cloud, const std::string &path); +/// @param opts Export settings; defaults to a binary triangulated mesh. +/// @return Number of faces written (0 when not triangulating). +size_t write_ply(const PointCloud &cloud, const std::string &path, + const PlyOptions &opts = PlyOptions{}); } // namespace score diff --git a/include/cloud_point/cpu_stereo_matcher.hpp b/include/cloud_point/cpu_stereo_matcher.hpp index 0ac32d2..0321d10 100644 --- a/include/cloud_point/cpu_stereo_matcher.hpp +++ b/include/cloud_point/cpu_stereo_matcher.hpp @@ -6,10 +6,32 @@ namespace score { /// @brief CPU-based stereo matcher using cv::StereoSGBM. +/// +/// The matcher is configured with the smoothness penalties and post-filters +/// recommended by OpenCV (P1 = 8·bs², P2 = 32·bs², uniqueness ratio, +/// left-right consistency check, speckle filter). Without them SGBM +/// degenerates to unregularised winner-take-all block matching, which +/// produces a heavy tail of low-disparity outliers that reproject metres +/// away from the true surface. class CpuStereoMatcher : public IStereoMatcher { public: + /// @brief SGBM tuning parameters. + struct Params { + int block_size; ///< Odd matching block size (SADWindowSize). + int uniqueness_ratio; ///< Best/second-best cost margin (%). + int speckle_window_size; ///< Max blob size flagged as speckle (0 off). + int speckle_range; ///< Max disparity variation inside a blob. + int disp12_max_diff; ///< Max left-right disparity mismatch (px). + int pre_filter_cap; ///< x-derivative clipping value. + // Explicit constructor avoids a GCC limitation with nested-struct + // default-member-initialisers used as default function arguments. + Params() noexcept + : block_size(5), uniqueness_ratio(10), speckle_window_size(100), + speckle_range(2), disp12_max_diff(1), pre_filter_cap(31) {} + }; + CpuStereoMatcher(int min_disparity = 0, int num_disparities = 128, - int block_size = 3); + Params params = Params{}); ~CpuStereoMatcher() override = default; [[nodiscard]] cv::Mat compute(const cv::Mat &left, diff --git a/include/cloud_point/gpu_stereo_matcher.hpp b/include/cloud_point/gpu_stereo_matcher.hpp index f09f90e..30b7050 100644 --- a/include/cloud_point/gpu_stereo_matcher.hpp +++ b/include/cloud_point/gpu_stereo_matcher.hpp @@ -6,15 +6,27 @@ namespace score { /// @brief GPU-based stereo matcher using cv::cuda::StereoSGM. /// Falls back to runtime error if CUDA is unavailable. +/// +/// cv::cuda::StereoSGM only supports 64, 128 or 256 disparity levels; the +/// requested count is rounded up to the next supported value. class GpuStereoMatcher : public IStereoMatcher { public: - GpuStereoMatcher(int min_disparity = 0, int num_disparities = 16, - int block_size = 3); + /// @param min_disparity Minimum disparity (px). + /// @param num_disparities Requested disparity levels (rounded up to + /// 64/128/256). + /// @param uniqueness_ratio Best/second-best cost margin (%). + GpuStereoMatcher(int min_disparity = 0, int num_disparities = 128, + int uniqueness_ratio = 10); ~GpuStereoMatcher() override = default; [[nodiscard]] cv::Mat compute(const cv::Mat &left, const cv::Mat &right) override; + /// @brief Round a disparity count up to the nearest value supported by + /// cv::cuda::StereoSGM (64, 128 or 256). + /// @throws std::invalid_argument if num_disparities exceeds 256. + [[nodiscard]] static int supported_num_disparities(int num_disparities); + private: #ifdef HAVE_OPENCV_CUDA cv::Ptr sgm_; diff --git a/include/cloud_point_rpc/cli.hpp b/include/cloud_point_rpc/cli.hpp index 6e83ed2..2c40bee 100644 --- a/include/cloud_point_rpc/cli.hpp +++ b/include/cloud_point_rpc/cli.hpp @@ -5,6 +5,19 @@ #include namespace score { +/** + * @brief Stereo reconstruction settings for CLI options 4 and 5. + * + * Mirrors CloudPointConfig from config.hpp without pulling OpenCV into the + * CLI interface. + */ +struct CliStereoOptions { + bool use_gpu{true}; ///< GPU matcher (falls back to CPU) or CPU + int num_disparities{128}; ///< SGBM levels, positive multiple of 16 + float min_depth_m{0.01f}; ///< Reject points nearer than this (m) + float max_depth_m{10.0f}; ///< Reject points farther than this (m) +}; + /** * @brief Runs the CLI client. * @@ -12,9 +25,11 @@ namespace score { * @param output Output stream (usually std::cout) * @param ip Server IP * @param port Server Port + * @param stereo Stereo reconstruction settings (options 4/5) * @return int exit code */ int CRPC_EXPORT run_cli(std::istream &input, std::ostream &output, - const std::string &ip, int port); + const std::string &ip, int port, + const CliStereoOptions &stereo = CliStereoOptions{}); } // namespace score diff --git a/include/cloud_point_rpc/config.hpp b/include/cloud_point_rpc/config.hpp index cd60cb2..ca94e7f 100644 --- a/include/cloud_point_rpc/config.hpp +++ b/include/cloud_point_rpc/config.hpp @@ -19,9 +19,23 @@ struct TestData { std::vector> cloud_point; }; +/// @brief Stereo reconstruction settings consumed by the CLI (option 4/5). +/// +/// Depth limits are physical bounds on the scene: anything reprojected +/// outside [min_depth_m, max_depth_m] is discarded as a mismatch. For the +/// SCARED endoscopic rig use roughly 0.02–0.3 m; the defaults are wide enough +/// for a generic Unity scene. +struct CloudPointConfig { + std::string algorithm{"gpu"}; ///< "gpu" (falls back to CPU) or "cpu" + int num_disparities{128}; ///< SGBM levels, positive multiple of 16 + double min_depth_m{0.01}; + double max_depth_m{10.0}; +}; + struct Config { ServerConfig server; TestData test_data; + CloudPointConfig cloud_point; }; class ConfigLoader { @@ -61,6 +75,31 @@ class ConfigLoader { LOG(WARNING) << "No 'test_data' section, using empty/defaults."; } + // Cloud point (optional) + if (config["cloud_point"]) { + const auto &cp = config["cloud_point"]; + CloudPointConfig d; + c.cloud_point.algorithm = + cp["algorithm"].as(d.algorithm); + c.cloud_point.num_disparities = + cp["num_disparities"].as(d.num_disparities); + c.cloud_point.min_depth_m = + cp["min_depth_m"].as(d.min_depth_m); + c.cloud_point.max_depth_m = + cp["max_depth_m"].as(d.max_depth_m); + if (c.cloud_point.algorithm != "gpu" && + c.cloud_point.algorithm != "cpu") { + throw std::runtime_error( + "cloud_point.algorithm must be \"gpu\" or \"cpu\""); + } + if (c.cloud_point.min_depth_m <= 0.0 || + c.cloud_point.max_depth_m <= c.cloud_point.min_depth_m) { + throw std::runtime_error( + "cloud_point depth range must satisfy " + "0 < min_depth_m < max_depth_m"); + } + } + return c; } catch (const YAML::Exception &e) { LOG(ERROR) << "Failed to load config: " << e.what(); diff --git a/src/cli.cpp b/src/cli.cpp index 2f3cb6f..a2e4f0d 100644 --- a/src/cli.cpp +++ b/src/cli.cpp @@ -41,7 +41,7 @@ std::string vector_to_string(const std::vector> &v) { } int run_cli(std::istream &input, std::ostream &output, const std::string &ip, - int port) { + int port, const CliStereoOptions &stereo) { try { TCPConnector connector(ip, port); RpcClient client(connector); @@ -86,7 +86,13 @@ int run_cli(std::istream &input, std::ostream &output, const std::string &ip, } else if (choice == "4" || choice == "5") { #ifdef HAVE_CLOUD_POINT_COMPUTE try { - CloudPointClient cpc(ip, port, StereoAlgorithmType::GPU); + const auto algo = stereo.use_gpu ? StereoAlgorithmType::GPU + : StereoAlgorithmType::CPU; + CloudPointClient cpc( + ip, port, algo, + PointCloudBuilder::Options{stereo.min_depth_m, + stereo.max_depth_m}, + stereo.num_disparities); cpc.connect(); auto result = cpc.compute_cloud(); @@ -127,9 +133,10 @@ int run_cli(std::istream &input, std::ostream &output, const std::string &ip, output << "PLY output path: "; std::string path; if (input >> path) { - write_ply(cloud, path); + const auto faces = write_ply(cloud, path); output << "Saved " << valid.size() - << " points to " << path << "\n"; + << " points and " << faces + << " faces to " << path << "\n"; } } } diff --git a/src/cloud_point/cloud_point_client.cpp b/src/cloud_point/cloud_point_client.cpp index 5c51de7..bd0cbc9 100644 --- a/src/cloud_point/cloud_point_client.cpp +++ b/src/cloud_point/cloud_point_client.cpp @@ -3,9 +3,14 @@ #include "cloud_point/imageFactory.h" #include "cloud_point_rpc/rpc_client.hpp" #include "cloud_point_rpc/tcp_connector.hpp" +#include +#include +#include +#include #include #include #include +#include namespace score { @@ -25,7 +30,7 @@ void CloudPointClient::connect() { const auto calib_rpc = client_->get_stereo_calibration(); const auto calib = StereoRectifier::Calibration::from_rpc(calib_rpc); rectifier_ = std::make_unique(calib); - matcher_ = StereoMatcherFactory::create(algo_, num_disparities_); + matcher_ = StereoMatcherFactory::create(algo_, num_disparities_); builder_ = std::make_unique(rectifier_->q(), opts_); } @@ -95,21 +100,124 @@ CloudPointClient::compute_cloud() { // PLY helper // --------------------------------------------------------------------------- -void write_ply(const PointCloud &cloud, const std::string &path) { - const auto valid = cloud.valid_points(); +namespace { - std::ofstream out(path); +struct Vertex { + float x, y, z; +}; + +/// Longest edge of a triangle must not exceed max_ratio * mean depth. +bool triangle_ok(const Vertex &a, const Vertex &b, const Vertex &c, + float max_ratio) { + const auto dist2 = [](const Vertex &p, const Vertex &q) { + const float dx = p.x - q.x, dy = p.y - q.y, dz = p.z - q.z; + return dx * dx + dy * dy + dz * dz; + }; + const float longest2 = std::max({dist2(a, b), dist2(b, c), dist2(c, a)}); + const float limit = max_ratio * (a.z + b.z + c.z) / 3.0f; + return longest2 <= limit * limit; +} + +} // namespace + +size_t write_ply(const PointCloud &cloud, const std::string &path, + const PlyOptions &opts) { + // Map every valid pixel to its index in the vertex list. + const size_t n_px = + static_cast(cloud.width) * static_cast(cloud.height); + std::vector index(n_px, -1); + std::vector vertices; + vertices.reserve(n_px); + for (size_t i = 0; i < n_px; ++i) { + const float x = cloud.data[i * 3]; + const float y = cloud.data[i * 3 + 1]; + const float z = cloud.data[i * 3 + 2]; + if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z)) { + index[i] = static_cast(vertices.size()); + vertices.push_back({x, y, z}); + } + } + + // Grid triangulation: each 2x2 pixel cell yields up to two triangles, + // wound so the normal faces the camera (-z in OpenCV coordinates). + std::vector> faces; + if (opts.triangulate) { + faces.reserve(2 * n_px); + const auto at = [&](int r, int c) { + return index[static_cast(r) * + static_cast(cloud.width) + + static_cast(c)]; + }; + const auto emit = [&](int i0, int i1, int i2) { + if (i0 < 0 || i1 < 0 || i2 < 0) + return; + if (triangle_ok(vertices[static_cast(i0)], + vertices[static_cast(i1)], + vertices[static_cast(i2)], + opts.max_edge_depth_ratio)) { + faces.push_back({i0, i1, i2}); + } + }; + for (int r = 0; r + 1 < cloud.height; ++r) { + for (int c = 0; c + 1 < cloud.width; ++c) { + const int i00 = at(r, c), i01 = at(r, c + 1); + const int i10 = at(r + 1, c), i11 = at(r + 1, c + 1); + const int valid = + (i00 >= 0) + (i01 >= 0) + (i10 >= 0) + (i11 >= 0); + if (valid == 4) { + emit(i00, i10, i11); + emit(i00, i11, i01); + } else if (valid == 3) { + // One missing corner: keep the single remaining triangle. + if (i00 < 0) + emit(i10, i11, i01); + else if (i01 < 0) + emit(i00, i10, i11); + else if (i10 < 0) + emit(i00, i11, i01); + else + emit(i00, i10, i01); + } + } + } + } + + std::ofstream out(path, std::ios::binary); out << "ply\n" - << "format ascii 1.0\n" - << "element vertex " << valid.size() << "\n" + << (opts.binary ? "format binary_little_endian 1.0\n" + : "format ascii 1.0\n") + << "element vertex " << vertices.size() << "\n" << "property float x\n" << "property float y\n" - << "property float z\n" - << "end_header\n"; - - for (const auto &pt : valid) { - out << pt[0] << " " << pt[1] << " " << pt[2] << "\n"; + << "property float z\n"; + if (opts.triangulate) { + out << "element face " << faces.size() << "\n" + << "property list uchar int vertex_indices\n"; } + out << "end_header\n"; + + if (opts.binary) { + static_assert(std::endian::native == std::endian::little, + "binary PLY writer assumes a little-endian host"); + static_assert(sizeof(Vertex) == 3 * sizeof(float)); + out.write( + reinterpret_cast(vertices.data()), + static_cast(vertices.size() * sizeof(Vertex))); + for (const auto &f : faces) { + const unsigned char count = 3; + out.write(reinterpret_cast(&count), 1); + out.write(reinterpret_cast(f.data()), + static_cast(3 * sizeof(int))); + } + } else { + for (const auto &v : vertices) { + out << v.x << " " << v.y << " " << v.z << "\n"; + } + for (const auto &f : faces) { + out << "3 " << f[0] << " " << f[1] << " " << f[2] << "\n"; + } + } + return faces.size(); } } // namespace score diff --git a/src/cloud_point/cpu_stereo_matcher.cpp b/src/cloud_point/cpu_stereo_matcher.cpp index d96500c..447f516 100644 --- a/src/cloud_point/cpu_stereo_matcher.cpp +++ b/src/cloud_point/cpu_stereo_matcher.cpp @@ -1,10 +1,28 @@ #include "cloud_point/cpu_stereo_matcher.hpp" +#include +#include + namespace score { CpuStereoMatcher::CpuStereoMatcher(int min_disparity, int num_disparities, - int block_size) { - sgbm_ = cv::StereoSGBM::create(min_disparity, num_disparities, block_size); + Params params) { + if (params.block_size < 1 || params.block_size % 2 == 0) { + throw std::invalid_argument( + "CpuStereoMatcher: block_size must be a positive odd number, got " + + std::to_string(params.block_size)); + } + + const int bs2 = params.block_size * params.block_size; + // OpenCV's recommended penalties for a single-channel input. + const int p1 = 8 * bs2; + const int p2 = 32 * bs2; + + sgbm_ = cv::StereoSGBM::create( + min_disparity, num_disparities, params.block_size, p1, p2, + params.disp12_max_diff, params.pre_filter_cap, params.uniqueness_ratio, + params.speckle_window_size, params.speckle_range, + cv::StereoSGBM::MODE_SGBM); } cv::Mat CpuStereoMatcher::compute(const cv::Mat &left, const cv::Mat &right) { diff --git a/src/cloud_point/gpu_stereo_matcher.cpp b/src/cloud_point/gpu_stereo_matcher.cpp index 3fd9c3a..135260e 100644 --- a/src/cloud_point/gpu_stereo_matcher.cpp +++ b/src/cloud_point/gpu_stereo_matcher.cpp @@ -1,5 +1,6 @@ #include "cloud_point/gpu_stereo_matcher.hpp" #include +#include #ifdef HAVE_OPENCV_CUDA #include @@ -8,18 +9,35 @@ namespace score { +int GpuStereoMatcher::supported_num_disparities(int num_disparities) { + if (num_disparities <= 64) + return 64; + if (num_disparities <= 128) + return 128; + if (num_disparities <= 256) + return 256; + throw std::invalid_argument( + "GpuStereoMatcher: cv::cuda::StereoSGM supports at most 256 " + "disparities, got " + + std::to_string(num_disparities)); +} + GpuStereoMatcher::GpuStereoMatcher(int min_disparity, int num_disparities, - int block_size) { + int uniqueness_ratio) { + const int levels = supported_num_disparities(num_disparities); #ifdef HAVE_OPENCV_CUDA if (cv::cuda::getCudaEnabledDeviceCount() == 0) { throw std::runtime_error("No CUDA devices available"); } - sgm_ = - cv::cuda::createStereoSGM(min_disparity, num_disparities, block_size); + // P1/P2 follow the cv::cuda::StereoSGM defaults (10/120), which are + // expressed on the census-transform cost scale rather than SAD. + sgm_ = cv::cuda::createStereoSGM(min_disparity, levels, /*P1=*/10, + /*P2=*/120, uniqueness_ratio, + cv::cuda::StereoSGM::MODE_HH4); #else (void)min_disparity; - (void)num_disparities; - (void)block_size; + (void)levels; + (void)uniqueness_ratio; throw std::runtime_error("OpenCV CUDA modules not available in this build"); #endif } diff --git a/src/cloud_point/stereo_matcher_factory.cpp b/src/cloud_point/stereo_matcher_factory.cpp index 92c356e..0bfab12 100644 --- a/src/cloud_point/stereo_matcher_factory.cpp +++ b/src/cloud_point/stereo_matcher_factory.cpp @@ -20,7 +20,7 @@ StereoMatcherFactory::create(StereoAlgorithmType type, int num_disparities) { return std::make_unique(0, num_disparities); case StereoAlgorithmType::GPU: try { - return std::make_unique(); + return std::make_unique(0, num_disparities); } catch (const std::exception &e) { LOG(WARNING) << "GPU stereo matcher unavailable: " << e.what() << ". Falling back to CPU."; diff --git a/src/main.cpp b/src/main.cpp index d1f3ba8..0c3061e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -25,8 +25,13 @@ int main(int argc, char *argv[]) { try { auto config = score::ConfigLoader::load(config_path); + score::CliStereoOptions stereo; + stereo.use_gpu = config.cloud_point.algorithm == "gpu"; + stereo.num_disparities = config.cloud_point.num_disparities; + stereo.min_depth_m = static_cast(config.cloud_point.min_depth_m); + stereo.max_depth_m = static_cast(config.cloud_point.max_depth_m); return score::run_cli(std::cin, std::cout, config.server.ip, - config.server.port); + config.server.port, stereo); } catch (const std::exception &e) { std::cerr << "Failed to start CLI: " << e.what() << std::endl; return 1; diff --git a/tests/meson.build b/tests/meson.build index 13d95af..406a83b 100644 --- a/tests/meson.build +++ b/tests/meson.build @@ -3,6 +3,7 @@ test_sources = files( 'test_integration.cpp', 'test_tcp.cpp', 'test_cli.cpp', + 'test_config.cpp', 'test_c_api.cpp', 'test_base64.cpp', 'test_serialize_image.cpp' @@ -20,6 +21,7 @@ if opencv_dep.found() 'test_point_cloud_builder.cpp', 'test_point_cloud_evaluator.cpp', 'test_cloud_point_client.cpp', + 'test_ply_export.cpp', 'test_scared_dataset.cpp', 'test_scared_ground_truth_loader.cpp' ) diff --git a/tests/test_config.cpp b/tests/test_config.cpp new file mode 100644 index 0000000..6b0794f --- /dev/null +++ b/tests/test_config.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include + +#include "cloud_point_rpc/config.hpp" + +namespace { + +struct TempConfig { + std::string path; + explicit TempConfig(const std::string &yaml) + : path("test_config_" + + std::to_string(reinterpret_cast(this)) + ".yaml") { + std::ofstream(path) << yaml; + } + ~TempConfig() { std::remove(path.c_str()); } +}; + +} // namespace + +TEST(ConfigLoaderTest, CloudPointSectionDefaultsWhenAbsent) { + TempConfig cfg("server:\n ip: \"127.0.0.1\"\n port: 8080\n"); + const auto c = score::ConfigLoader::load(cfg.path); + EXPECT_EQ(c.cloud_point.algorithm, "gpu"); + EXPECT_EQ(c.cloud_point.num_disparities, 128); + EXPECT_DOUBLE_EQ(c.cloud_point.min_depth_m, 0.01); + EXPECT_DOUBLE_EQ(c.cloud_point.max_depth_m, 10.0); +} + +TEST(ConfigLoaderTest, CloudPointSectionIsParsed) { + TempConfig cfg("server:\n ip: \"127.0.0.1\"\n port: 8080\n" + "cloud_point:\n algorithm: cpu\n num_disparities: 160\n" + " min_depth_m: 0.02\n max_depth_m: 0.3\n"); + const auto c = score::ConfigLoader::load(cfg.path); + EXPECT_EQ(c.cloud_point.algorithm, "cpu"); + EXPECT_EQ(c.cloud_point.num_disparities, 160); + EXPECT_DOUBLE_EQ(c.cloud_point.min_depth_m, 0.02); + EXPECT_DOUBLE_EQ(c.cloud_point.max_depth_m, 0.3); +} + +TEST(ConfigLoaderTest, CloudPointPartialSectionKeepsDefaults) { + TempConfig cfg("server:\n ip: \"127.0.0.1\"\n port: 8080\n" + "cloud_point:\n max_depth_m: 0.5\n"); + const auto c = score::ConfigLoader::load(cfg.path); + EXPECT_EQ(c.cloud_point.algorithm, "gpu"); + EXPECT_EQ(c.cloud_point.num_disparities, 128); + EXPECT_DOUBLE_EQ(c.cloud_point.max_depth_m, 0.5); +} + +TEST(ConfigLoaderTest, CloudPointRejectsBadAlgorithm) { + TempConfig cfg("server:\n ip: \"127.0.0.1\"\n port: 8080\n" + "cloud_point:\n algorithm: fpga\n"); + EXPECT_THROW(std::ignore = score::ConfigLoader::load(cfg.path), + std::runtime_error); +} + +TEST(ConfigLoaderTest, CloudPointRejectsInvertedDepthRange) { + TempConfig cfg("server:\n ip: \"127.0.0.1\"\n port: 8080\n" + "cloud_point:\n min_depth_m: 1.0\n max_depth_m: 0.5\n"); + EXPECT_THROW(std::ignore = score::ConfigLoader::load(cfg.path), + std::runtime_error); +} diff --git a/tests/test_ply_export.cpp b/tests/test_ply_export.cpp new file mode 100644 index 0000000..4efd512 --- /dev/null +++ b/tests/test_ply_export.cpp @@ -0,0 +1,123 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cloud_point/cloud_point_client.hpp" + +using namespace score; + +namespace { + +/// 3x3 organised cloud on a plane z = 1 m with 1 cm pixel spacing. +PointCloud make_grid_cloud() { + PointCloud cloud; + cloud.width = 3; + cloud.height = 3; + cloud.data.resize(27); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + const size_t i = static_cast(r * 3 + c) * 3; + cloud.data[i] = 0.01f * static_cast(c); + cloud.data[i + 1] = 0.01f * static_cast(r); + cloud.data[i + 2] = 1.0f; + } + } + return cloud; +} + +void set_nan(PointCloud &cloud, int r, int c) { + const size_t i = static_cast(r * cloud.width + c) * 3; + const float nan = std::numeric_limits::quiet_NaN(); + cloud.data[i] = cloud.data[i + 1] = cloud.data[i + 2] = nan; +} + +struct TempFile { + std::string path{"test_ply_export.ply"}; + ~TempFile() { std::remove(path.c_str()); } + std::string read() const { + std::ifstream in(path); + std::stringstream ss; + ss << in.rdbuf(); + return ss.str(); + } +}; + +} // namespace + +TEST(PlyExportTest, PointsOnlyHasNoFaceElement) { + TempFile file; + const auto faces = + write_ply(make_grid_cloud(), file.path, PlyOptions{false, 0.05f}); + EXPECT_EQ(faces, 0u); + const auto text = file.read(); + EXPECT_NE(text.find("element vertex 9\n"), std::string::npos); + EXPECT_EQ(text.find("element face"), std::string::npos); +} + +TEST(PlyExportTest, FullGridProducesTwoTrianglesPerCell) { + TempFile file; + const auto faces = + write_ply(make_grid_cloud(), file.path, PlyOptions{true, 0.05f, false}); + EXPECT_EQ(faces, 8u); // 2x2 cells * 2 triangles + EXPECT_NE(file.read().find("format ascii 1.0\n"), std::string::npos); + const auto text = file.read(); + EXPECT_NE(text.find("element face 8\n"), std::string::npos); + EXPECT_NE(text.find("property list uchar int vertex_indices\n"), + std::string::npos); + // First cell, first triangle: (0,0) -> (1,0) -> (1,1) = indices 0,3,4 + EXPECT_NE(text.find("\n3 0 3 4\n"), std::string::npos); +} + +TEST(PlyExportTest, MissingCornerKeepsSingleTriangle) { + auto cloud = make_grid_cloud(); + set_nan(cloud, 0, 0); // top-left cell has 3 valid corners + TempFile file; + const auto faces = + write_ply(cloud, file.path, PlyOptions{true, 0.05f, false}); + EXPECT_EQ(faces, 7u); + const auto text = file.read(); + EXPECT_NE(text.find("element vertex 8\n"), std::string::npos); +} + +TEST(PlyExportTest, DepthJumpBreaksMesh) { + auto cloud = make_grid_cloud(); + // Push the centre column 20 cm away: every triangle touching it now has + // an edge longer than 5% of its mean depth and must be dropped. + for (int r = 0; r < 3; ++r) { + cloud.data[static_cast(r * 3 + 1) * 3 + 2] = 1.2f; + } + TempFile file; + EXPECT_EQ(write_ply(cloud, file.path), 0u); +} + +TEST(PlyExportTest, BinaryLayoutMatchesHeader) { + TempFile file; + const auto faces = write_ply(make_grid_cloud(), file.path); // binary + ASSERT_EQ(faces, 8u); + const auto text = file.read(); + EXPECT_NE(text.find("format binary_little_endian 1.0\n"), + std::string::npos); + const auto header_end = text.find("end_header\n") + 11; + ASSERT_NE(header_end, std::string::npos + 11); + // 9 vertices * 12 bytes + 8 faces * (1 + 12) bytes + EXPECT_EQ(text.size() - header_end, 9u * 12u + 8u * 13u); + // First vertex is (0, 0, 1) + float v[3]; + std::memcpy(v, text.data() + header_end, sizeof(v)); + EXPECT_FLOAT_EQ(v[0], 0.0f); + EXPECT_FLOAT_EQ(v[1], 0.0f); + EXPECT_FLOAT_EQ(v[2], 1.0f); + // First face: count byte 3 then indices 0,3,4 + const char *f = text.data() + header_end + 9 * 12; + EXPECT_EQ(static_cast(f[0]), 3u); + int idx[3]; + std::memcpy(idx, f + 1, sizeof(idx)); + EXPECT_EQ(idx[0], 0); + EXPECT_EQ(idx[1], 3); + EXPECT_EQ(idx[2], 4); +} diff --git a/tests/test_stereo_matcher.cpp b/tests/test_stereo_matcher.cpp index cd2cec2..2982958 100644 --- a/tests/test_stereo_matcher.cpp +++ b/tests/test_stereo_matcher.cpp @@ -1,5 +1,7 @@ +#include #include #include +#include #include #include "cloud_point/cpu_stereo_matcher.hpp" @@ -47,9 +49,54 @@ TEST(StereoMatcherTest, FactoryCpuCreatesNonNull) { EXPECT_FALSE(disparity.empty()); } +TEST(StereoMatcherTest, CpuMatcherRejectsEvenBlockSize) { + CpuStereoMatcher::Params params; + params.block_size = 4; + EXPECT_THROW(CpuStereoMatcher(0, 64, params), std::invalid_argument); +} + +TEST(StereoMatcherTest, CpuMatcherRecoversKnownShift) { + // Textured synthetic pair: right image is the left shifted by 8 px, so + // the (16x fixed-point) disparity in the interior should be ~8 px. + constexpr int kShift = 8; + cv::Mat left(120, 200, CV_8UC1); + cv::randu(left, 0, 255); + cv::blur(left, left, cv::Size(3, 3)); + cv::Mat right = cv::Mat::zeros(left.size(), CV_8UC1); + left(cv::Rect(kShift, 0, left.cols - kShift, left.rows)) + .copyTo(right(cv::Rect(0, 0, left.cols - kShift, left.rows))); + + CpuStereoMatcher matcher(0, 64); + cv::Mat disparity = matcher.compute(left, right); + ASSERT_EQ(disparity.type(), CV_16S); + + int good = 0, total = 0; + for (int y = 10; y < left.rows - 10; ++y) { + for (int x = 70; x < left.cols - 20; ++x) { + const float d = disparity.at(y, x) / 16.0f; + if (d <= 0) + continue; + ++total; + if (std::abs(d - kShift) <= 1.0f) + ++good; + } + } + ASSERT_GT(total, 0); + EXPECT_GT(static_cast(good) / total, 0.9); +} + +TEST(StereoMatcherTest, GpuSupportedNumDisparitiesRoundsUp) { + EXPECT_EQ(GpuStereoMatcher::supported_num_disparities(16), 64); + EXPECT_EQ(GpuStereoMatcher::supported_num_disparities(64), 64); + EXPECT_EQ(GpuStereoMatcher::supported_num_disparities(96), 128); + EXPECT_EQ(GpuStereoMatcher::supported_num_disparities(160), 256); + EXPECT_THROW(std::ignore = GpuStereoMatcher::supported_num_disparities(272), + std::invalid_argument); +} + TEST(StereoMatcherTest, FactoryRejectsInvalidNumDisparities) { - EXPECT_THROW(std::ignore = StereoMatcherFactory::create( - StereoAlgorithmType::CPU, 0), + EXPECT_THROW(std::ignore = + StereoMatcherFactory::create(StereoAlgorithmType::CPU, 0), std::invalid_argument); EXPECT_THROW(std::ignore = StereoMatcherFactory::create( StereoAlgorithmType::CPU, -16),