fix(cloud_point): regularise SGBM and export viewable PLY meshes
- Configure StereoSGBM with P1/P2 penalties, 5x5 block, uniqueness ratio, speckle filter and left-right check; previously it ran unregularised and produced a heavy tail of low-disparity outliers reprojecting metres away (SCARED benchmark MAE3D 44 mm -> 0.8 mm, RMSE 289 mm -> 1.4 mm) - Pass the requested disparity count to the CUDA SGM matcher (rounded to 64/128/256) instead of a hard-coded 16 - Add optional `cloud_point` config section (algorithm, num_disparities, depth range) consumed by CLI options 4/5; ship config.scared.yml - write_ply now emits a binary PLY with a grid-triangulated mesh so web viewers stop fabricating slivers from consecutive vertices - Add config, matcher and PLY export tests; document in README
This commit is contained in:
parent
cd97e7b3f1
commit
4f21e2ea38
3
.gitignore
vendored
3
.gitignore
vendored
@ -17,3 +17,6 @@ large_tool_results/
|
||||
|
||||
# IDE
|
||||
.idea/
|
||||
|
||||
# Point-cloud exports
|
||||
*.ply
|
||||
|
||||
62
README.md
62
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
|
||||
|
||||
|
||||
11
config.scared.yml
Normal file
11
config.scared.yml
Normal file
@ -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
|
||||
@ -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
|
||||
|
||||
@ -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<PointCloudBuilder> 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
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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<cv::cuda::StereoSGM> sgm_;
|
||||
|
||||
@ -5,6 +5,19 @@
|
||||
#include <string>
|
||||
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
|
||||
|
||||
@ -19,9 +19,23 @@ struct TestData {
|
||||
std::vector<std::vector<double>> 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<std::string>(d.algorithm);
|
||||
c.cloud_point.num_disparities =
|
||||
cp["num_disparities"].as<int>(d.num_disparities);
|
||||
c.cloud_point.min_depth_m =
|
||||
cp["min_depth_m"].as<double>(d.min_depth_m);
|
||||
c.cloud_point.max_depth_m =
|
||||
cp["max_depth_m"].as<double>(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();
|
||||
|
||||
15
src/cli.cpp
15
src/cli.cpp
@ -41,7 +41,7 @@ std::string vector_to_string(const std::vector<std::vector<T>> &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";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -3,9 +3,14 @@
|
||||
#include "cloud_point/imageFactory.h"
|
||||
#include "cloud_point_rpc/rpc_client.hpp"
|
||||
#include "cloud_point_rpc/tcp_connector.hpp"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <bit>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <jsonrpccxx/common.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <vector>
|
||||
|
||||
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<StereoRectifier>(calib);
|
||||
matcher_ = StereoMatcherFactory::create(algo_, num_disparities_);
|
||||
matcher_ = StereoMatcherFactory::create(algo_, num_disparities_);
|
||||
builder_ = std::make_unique<PointCloudBuilder>(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<size_t>(cloud.width) * static_cast<size_t>(cloud.height);
|
||||
std::vector<int> index(n_px, -1);
|
||||
std::vector<Vertex> 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<int>(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<std::array<int, 3>> faces;
|
||||
if (opts.triangulate) {
|
||||
faces.reserve(2 * n_px);
|
||||
const auto at = [&](int r, int c) {
|
||||
return index[static_cast<size_t>(r) *
|
||||
static_cast<size_t>(cloud.width) +
|
||||
static_cast<size_t>(c)];
|
||||
};
|
||||
const auto emit = [&](int i0, int i1, int i2) {
|
||||
if (i0 < 0 || i1 < 0 || i2 < 0)
|
||||
return;
|
||||
if (triangle_ok(vertices[static_cast<size_t>(i0)],
|
||||
vertices[static_cast<size_t>(i1)],
|
||||
vertices[static_cast<size_t>(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<const char *>(vertices.data()),
|
||||
static_cast<std::streamsize>(vertices.size() * sizeof(Vertex)));
|
||||
for (const auto &f : faces) {
|
||||
const unsigned char count = 3;
|
||||
out.write(reinterpret_cast<const char *>(&count), 1);
|
||||
out.write(reinterpret_cast<const char *>(f.data()),
|
||||
static_cast<std::streamsize>(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
|
||||
|
||||
@ -1,10 +1,28 @@
|
||||
#include "cloud_point/cpu_stereo_matcher.hpp"
|
||||
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
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) {
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "cloud_point/gpu_stereo_matcher.hpp"
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#ifdef HAVE_OPENCV_CUDA
|
||||
#include <opencv2/cudaimgproc.hpp>
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -20,7 +20,7 @@ StereoMatcherFactory::create(StereoAlgorithmType type, int num_disparities) {
|
||||
return std::make_unique<CpuStereoMatcher>(0, num_disparities);
|
||||
case StereoAlgorithmType::GPU:
|
||||
try {
|
||||
return std::make_unique<GpuStereoMatcher>();
|
||||
return std::make_unique<GpuStereoMatcher>(0, num_disparities);
|
||||
} catch (const std::exception &e) {
|
||||
LOG(WARNING) << "GPU stereo matcher unavailable: " << e.what()
|
||||
<< ". Falling back to CPU.";
|
||||
|
||||
@ -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<float>(config.cloud_point.min_depth_m);
|
||||
stereo.max_depth_m = static_cast<float>(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;
|
||||
|
||||
@ -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'
|
||||
)
|
||||
|
||||
63
tests/test_config.cpp
Normal file
63
tests/test_config.cpp
Normal file
@ -0,0 +1,63 @@
|
||||
#include <cstdio>
|
||||
#include <fstream>
|
||||
#include <gtest/gtest.h>
|
||||
#include <string>
|
||||
|
||||
#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<uintptr_t>(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);
|
||||
}
|
||||
123
tests/test_ply_export.cpp
Normal file
123
tests/test_ply_export.cpp
Normal file
@ -0,0 +1,123 @@
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
#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<size_t>(r * 3 + c) * 3;
|
||||
cloud.data[i] = 0.01f * static_cast<float>(c);
|
||||
cloud.data[i + 1] = 0.01f * static_cast<float>(r);
|
||||
cloud.data[i + 2] = 1.0f;
|
||||
}
|
||||
}
|
||||
return cloud;
|
||||
}
|
||||
|
||||
void set_nan(PointCloud &cloud, int r, int c) {
|
||||
const size_t i = static_cast<size_t>(r * cloud.width + c) * 3;
|
||||
const float nan = std::numeric_limits<float>::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<size_t>(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<unsigned char>(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);
|
||||
}
|
||||
@ -1,5 +1,7 @@
|
||||
#include <cmath>
|
||||
#include <gtest/gtest.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <tuple>
|
||||
|
||||
#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<short>(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<double>(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),
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user