feat(cloud_point): stereo rectification, point cloud pipeline, and CloudPointClient facade
- StereoRectifier wrapping cv::stereoRectify + cv::initUndistortRectifyMap (CV_16SC2 maps) - PointCloudBuilder with cv::reprojectImageTo3D and depth/NaN filtering - CloudPointClient high-level facade: connect(), compute_cloud() -> std::expected<PointCloud, Error> - write_ply() ASCII PLY export helper - CLI options 4 (compute-cloud) and 5 (compute-cloud + save PLY) - Fix TcpServer to loop over multiple requests per connection - Expose num_disparities parameter through CloudPointClient and StereoMatcherFactory - Unity C# integration design spec - Sync README, AGENTS, openwiki with stereo pipeline (C++23) - E2E synthetic-scene test with constant-disparity stereo pair TG-9 #ready-for-test TG-4 #ready-for-test TG-2 #in-progress
This commit is contained in:
parent
d486f550d2
commit
162c210a7d
21
AGENTS.md
21
AGENTS.md
@ -1,6 +1,6 @@
|
||||
# Cloud Point RPC Agent Guide
|
||||
|
||||
This repository contains a C++20 implementation of a JSON RPC protocol for communicating with a Unity Scene.
|
||||
This repository contains a C++23 implementation of a JSON RPC protocol for communicating with a Unity Scene.
|
||||
Agents working on this codebase must adhere to the following guidelines and conventions.
|
||||
|
||||
## 1. Build, Lint, and Test
|
||||
@ -54,10 +54,10 @@ The project uses the **Meson** build system.
|
||||
|
||||
## 2. Code Style & Conventions
|
||||
|
||||
Adhere strictly to **Modern C++20** standards.
|
||||
Adhere strictly to **Modern C++23** standards.
|
||||
|
||||
### General Guidelines
|
||||
- **Standard:** C++20. Use concepts, ranges, and smart pointers. Avoid raw `new`/`delete`.
|
||||
- **Standard:** C++23. Use concepts, ranges, `std::expected`, and smart pointers. Avoid raw `new`/`delete`.
|
||||
- **Memory Management:** Use `std::unique_ptr` and `std::shared_ptr`.
|
||||
- **Const Correctness:** Use `const` (and `constexpr`/`consteval`) whenever possible.
|
||||
- **Includes:** Use absolute paths for project headers (e.g., `#include "rpc/server.hpp"`).
|
||||
@ -74,8 +74,10 @@ Adhere strictly to **Modern C++20** standards.
|
||||
- **Interfaces:** `IPascalCase`.
|
||||
|
||||
### Project Structure
|
||||
- `include/cloud_point_rpc/`: Public header files.
|
||||
- `src/`: Implementation files.
|
||||
- `include/cloud_point_rpc/`: Public header files (RPC server/client, TCP, config, serialization, coder, DTOs).
|
||||
- `include/cloud_point/`: OpenCV compute library headers (StereoRectifier, PointCloudBuilder, CloudPointClient). Optional; requires opencv4.
|
||||
- `src/`: Implementation files and executable entrypoints.
|
||||
- `src/cloud_point/`: OpenCV compute library implementation (optional, requires opencv4).
|
||||
- `tests/`: Unit and integration tests.
|
||||
- `subprojects/`: Meson wrap files for dependencies.
|
||||
- `meson.build`: Build configuration.
|
||||
@ -120,8 +122,13 @@ class CameraController {
|
||||
### Implementation Details
|
||||
- **JSON Library:** Use `nlohmann/json` (likely via `subprojects/nlohmann_json.wrap`).
|
||||
- **Concurrency:** Use `std::jthread` (auto-joining) over `std::thread`.
|
||||
- **RPC Methods:**
|
||||
- Implement handlers for: `get-cloud-point`, `get-intrinsic-params`, `get-extrinsic-params`.
|
||||
- **Error handling:** Use `std::expected<T, E>` (C++23) for recoverable errors in the `cloud_point` compute library.
|
||||
- **RPC Methods** (served by Unity or the C++ mock in `server_main.cpp`):
|
||||
- `get-available-methods` — list registered method names.
|
||||
- `get-stereo-calibration` — full stereo rig calibration (intrinsics, R, T, image size).
|
||||
- `get-image-pair` — synchronised stereo frame as two base64-encoded images.
|
||||
- `get-intrinsic-params` *(legacy)* — left-camera intrinsic matrix (9 doubles).
|
||||
- `get-extrinsic-params` *(legacy)* — left-camera extrinsic matrix (16 doubles).
|
||||
- Ensure thread safety if the RPC server is multi-threaded.
|
||||
|
||||
## 3. Workflow & Git
|
||||
|
||||
34
README.md
34
README.md
@ -13,23 +13,29 @@ Communication JSON RPC protocol and implementation with Unity Scene.
|
||||
## Status
|
||||
|
||||
- [x] Server implementation with C-API for Unity
|
||||
- [x] Basic OpenCV image processing (Rectification, Image wrapper)
|
||||
- [ ] Full OpenCV client implementation
|
||||
- [x] OpenCV stereo client (StereoRectifier, PointCloudBuilder, CloudPointClient facade)
|
||||
- [ ] Unity-side C# implementation per [docs/unity-integration.md](docs/unity-integration.md)
|
||||
|
||||
## API Documentation
|
||||
|
||||
See [API.md](API.md) for detailed request/response formats.
|
||||
|
||||
## Pipeline
|
||||
|
||||
Unity acts as a data source: it serves stereo image pairs (`get-image-pair`) and full stereo calibration (`get-stereo-calibration`) over JSON-RPC 2.0. The C++ `CloudPointClient` calls `connect()` once to fetch calibration, then on each `compute_cloud()` call it fetches a synchronised image pair, runs stereo rectification (`StereoRectifier`, cv::stereoRectify + remap), computes disparity with SGBM (16× scaling), reprojects to 3-D with `cv::reprojectImageTo3D` (`PointCloudBuilder`), filters NaN/invalid points, and returns a `PointCloud`. An optional `write_ply()` helper serialises the result to disk.
|
||||
|
||||
See [API.md](API.md) for wire schemas and [docs/unity-integration.md](docs/unity-integration.md) for the Unity C# design spec.
|
||||
|
||||
## Development
|
||||
|
||||
The project uses **Meson** build system and **C++20**.
|
||||
The project uses **Meson** build system and **C++23**.
|
||||
|
||||
### Dependencies
|
||||
|
||||
- Meson (>= 1.1.0), Ninja
|
||||
- GCC/Clang (C++20 support)
|
||||
- GCC/Clang (C++23 support)
|
||||
- Git (for subprojects)
|
||||
- OpenCV (for cloud point compute)
|
||||
- OpenCV 4 (optional; required for stereo point cloud compute)
|
||||
|
||||
The following dependencies are managed via Meson subprojects:
|
||||
- [ASIO](https://think-async.com/Asio/) (Networking)
|
||||
@ -37,7 +43,6 @@ The following dependencies are managed via Meson subprojects:
|
||||
- [yaml-cpp](https://github.com/jbeder/yaml-cpp) (Configuration loading)
|
||||
- [glog](https://github.com/google/glog) (Logging)
|
||||
- [jsonrpccxx](https://github.com/uS-S/jsonrpccxx) (JSON-RPC 2.0 implementation)
|
||||
- [stdexec](https://github.com/NVIDIA/stdexec) (P2300 Senders/Receivers)
|
||||
|
||||
### Build & Run
|
||||
|
||||
@ -49,6 +54,23 @@ meson compile -C build
|
||||
|
||||
*Note: You need a `config.yaml` file. See `config.yaml.example` for the required format.*
|
||||
|
||||
Run the interactive CLI client:
|
||||
|
||||
```bash
|
||||
./build/src/cloud_point_rpc_cli config.yaml
|
||||
```
|
||||
|
||||
CLI menu options (OpenCV options are hidden when built without opencv4):
|
||||
|
||||
| Option | Action |
|
||||
|--------|--------|
|
||||
| 1 | List available RPC methods |
|
||||
| 2 | Get intrinsic params (legacy) |
|
||||
| 3 | Get extrinsic params (legacy) |
|
||||
| 4 | Compute point cloud — prints point count and bounding box |
|
||||
| 5 | Compute point cloud and save to `output.ply` |
|
||||
| 0 | Exit |
|
||||
|
||||
#### Build on windows
|
||||
|
||||
It's assumed that you have `GCC` and `make`/`ninja` installed on your system (and available in `PATH`)
|
||||
|
||||
@ -2,65 +2,81 @@
|
||||
box ClientProcess #LightBlue
|
||||
Participant Caller
|
||||
Participant CloudPointClient
|
||||
Participant StereoRectifier
|
||||
Participant PointCloudBuilder
|
||||
Participant TCPClient
|
||||
end box
|
||||
box UnityProcess #LightGreen
|
||||
Participant TCPServer
|
||||
Participant CloudPointServer
|
||||
Participant MainThreadQueue
|
||||
Participant UnityWorld
|
||||
end box
|
||||
UnityWorld -> CloudPointServer : init thread
|
||||
activate CloudPointServer
|
||||
CloudPointServer -> TCPServer : await for connection
|
||||
activate TCPServer
|
||||
->CloudPointClient : init thread
|
||||
activate CloudPointClient
|
||||
CloudPointClient -> TCPClient : createConnection
|
||||
TCPClient -> TCPServer : establish connection
|
||||
TCPServer -> CloudPointServer : established
|
||||
deactivate TCPServer
|
||||
CloudPointServer -> TCPServer : await for calls
|
||||
|
||||
TCPServer -> TCPServer : await for packet
|
||||
Caller -> CloudPointClient : I want something
|
||||
activate CloudPointClient
|
||||
CloudPointClient -> CloudPointClient : CallMethod<Something>
|
||||
CloudPointClient -> TCPClient : send(message)
|
||||
activate TCPClient
|
||||
TCPClient -> TCPServer : packet send
|
||||
TCPServer -> TCPServer : await for packet
|
||||
activate TCPServer
|
||||
TCPServer -> TCPServer : read packet
|
||||
TCPServer -> TCPClient : packet read
|
||||
TCPClient -> CloudPointClient : done
|
||||
deactivate TCPClient
|
||||
CloudPointClient -> TCPClient : await for response
|
||||
activate TCPClient
|
||||
TCPClient -> TCPClient : await for packet
|
||||
TCPServer -> CloudPointServer : callMethod
|
||||
activate CloudPointServer
|
||||
CloudPointServer -> UnityWorld : addToStaticQueue
|
||||
== Initialization ==
|
||||
|
||||
UnityWorld -> UnityWorld : read from queue
|
||||
UnityWorld -> CloudPointServer : Awake() — crpc_init()
|
||||
activate CloudPointServer
|
||||
CloudPointServer -> TCPServer : start (await connections)
|
||||
activate TCPServer
|
||||
|
||||
Caller -> CloudPointClient : connect()
|
||||
activate CloudPointClient
|
||||
CloudPointClient -> TCPClient : establish TCP connection
|
||||
TCPClient -> TCPServer : TCP handshake
|
||||
TCPServer -> TCPServer : spawn per-client handler thread (loops)
|
||||
|
||||
CloudPointClient -> TCPClient : get-stereo-calibration
|
||||
TCPClient -> TCPServer : send request
|
||||
TCPServer -> CloudPointServer : dispatch
|
||||
CloudPointServer -> MainThreadQueue : enqueue task
|
||||
MainThreadQueue -> UnityWorld : Update() dequeues
|
||||
activate UnityWorld
|
||||
UnityWorld -> UnityWorld : callMethod
|
||||
UnityWorld -> CloudPointServer: set task return value
|
||||
UnityWorld -> UnityWorld : read camera params
|
||||
UnityWorld -> MainThreadQueue : tcs.SetResult(calibration JSON)
|
||||
deactivate UnityWorld
|
||||
CloudPointServer -> TCPServer : return task
|
||||
deactivate CloudPointServer
|
||||
TCPServer -> TCPClient : send response
|
||||
TCPClient -> TCPServer : response read
|
||||
TCPClient -> CloudPointClient : response received
|
||||
TCPServer -> CloudPointServer : done
|
||||
deactivate TCPServer
|
||||
CloudPointClient -> Caller : here what you wanted
|
||||
MainThreadQueue -> CloudPointServer : task complete
|
||||
CloudPointServer -> TCPServer : send response
|
||||
TCPServer -> TCPClient : response
|
||||
TCPClient -> CloudPointClient : calibration received
|
||||
CloudPointClient -> StereoRectifier : init(calibration)\ncv::stereoRectify + remap maps
|
||||
deactivate CloudPointClient
|
||||
|
||||
Caller -> CloudPointClient : destruct
|
||||
CloudPointClient -> TCPClient : finish waiting
|
||||
deactivate TCPClient
|
||||
== Per compute_cloud() call ==
|
||||
|
||||
Caller -> CloudPointClient : compute_cloud()
|
||||
activate CloudPointClient
|
||||
CloudPointClient -> TCPClient : get-image-pair
|
||||
TCPClient -> TCPServer : send request
|
||||
TCPServer -> CloudPointServer : dispatch
|
||||
CloudPointServer -> MainThreadQueue : enqueue task
|
||||
MainThreadQueue -> UnityWorld : Update() dequeues
|
||||
activate UnityWorld
|
||||
UnityWorld -> UnityWorld : Render() both cameras\nReadPixels + vertical flip\nBase64 encode
|
||||
UnityWorld -> MainThreadQueue : tcs.SetResult(image pair JSON)
|
||||
deactivate UnityWorld
|
||||
MainThreadQueue -> CloudPointServer : task complete
|
||||
CloudPointServer -> TCPServer : send response
|
||||
TCPServer -> TCPClient : response (base64 left + right)
|
||||
TCPClient -> CloudPointClient : image pair received
|
||||
|
||||
CloudPointClient -> StereoRectifier : rectify(left, right)
|
||||
StereoRectifier -> StereoRectifier : cv::remap both images
|
||||
StereoRectifier -> CloudPointClient : rectified pair
|
||||
|
||||
CloudPointClient -> PointCloudBuilder : build(rectified, Q)
|
||||
PointCloudBuilder -> PointCloudBuilder : cv::StereoSGBM disparity\n(1/16 scale)\ncv::reprojectImageTo3D\nNaN filter
|
||||
PointCloudBuilder -> CloudPointClient : PointCloud
|
||||
|
||||
CloudPointClient -> Caller : std::expected<PointCloud, Error>
|
||||
deactivate CloudPointClient
|
||||
UnityWorld -> CloudPointServer : destruct
|
||||
|
||||
== Teardown ==
|
||||
|
||||
Caller -> CloudPointClient : destruct
|
||||
deactivate CloudPointClient
|
||||
UnityWorld -> CloudPointServer : OnDestroy() — crpc_deinit()
|
||||
deactivate CloudPointServer
|
||||
deactivate TCPServer
|
||||
|
||||
@enduml
|
||||
|
||||
198
docs/unity-integration.md
Normal file
198
docs/unity-integration.md
Normal file
@ -0,0 +1,198 @@
|
||||
# Unity Integration Design
|
||||
|
||||
This document specifies the C# architecture the Unity side (the
|
||||
`UnityLaparoscopicSceneSimulator` project) must implement to serve stereo
|
||||
images and calibration to the C++ `CloudPointClient` over the embedded RPC
|
||||
server. It is a design spec — the C# code lives in the Unity repository.
|
||||
|
||||
The existing prototypes there (`Assets/Scripts/CrpcApi.cs`,
|
||||
`Assets/Scripts/RpcTest.cs`) are a starting point but contain several
|
||||
correctness bugs called out explicitly below.
|
||||
|
||||
## Data flow
|
||||
|
||||
```
|
||||
Unity process (server) C++ client process
|
||||
────────────────────── ──────────────────
|
||||
CloudPointServer (C#)
|
||||
crpc_init / crpc_add_method
|
||||
get-stereo-calibration ◄────────────── CloudPointClient::connect() (once)
|
||||
get-image-pair ◄────────────── CloudPointClient::compute_cloud()
|
||||
│ rectify → SGBM → reproject
|
||||
▼
|
||||
PointCloud → caller
|
||||
```
|
||||
|
||||
Unity is a **data source only**. The point cloud is computed and consumed on
|
||||
the client side. See [API.md](../API.md) for the wire schemas.
|
||||
|
||||
## 1. `CloudPointServer` facade
|
||||
|
||||
A single MonoBehaviour owning the server lifecycle
|
||||
(`Assets/Scripts/CloudPointRpc/CloudPointServer.cs`):
|
||||
|
||||
- `Awake()` — call `crpc_init(configPath)` (config.yaml under
|
||||
`Application.streamingAssetsPath`, contains `server: {ip, port}`), then
|
||||
register handlers.
|
||||
- `OnDestroy()` / `OnApplicationQuit()` — shut down in the order specified in
|
||||
§3 (Shutdown ordering).
|
||||
- JSON: use Newtonsoft Json.NET (`com.unity.nuget.newtonsoft-json`).
|
||||
`JsonUtility` cannot serialize dictionaries or nested arrays.
|
||||
|
||||
### Callback registration rules (IL2CPP + GC safety)
|
||||
|
||||
- Handler methods must be **static** and annotated with
|
||||
`[AOT.MonoPInvokeCallback(typeof(RpcStringCallback))]` — instance methods
|
||||
crash under IL2CPP.
|
||||
- The delegate instance passed to `crpc_add_method` must be stored in a
|
||||
**static field** for the lifetime of the server. The current `RpcTest.cs`
|
||||
passes a method group directly; the marshalled thunk can be garbage
|
||||
collected while C++ still holds the function pointer — a latent crash.
|
||||
|
||||
```csharp
|
||||
private static readonly CrpcTestApi.RpcStringCallback s_calibrationCb = OnGetStereoCalibration;
|
||||
// ...
|
||||
CrpcExtensions.CrpcAddMethod(s_calibrationCb, "get-stereo-calibration");
|
||||
```
|
||||
|
||||
## 2. `rpc_string` ownership rules
|
||||
|
||||
The C API (`server_api.h`) uses `rpc_string*` in both directions with
|
||||
**different ownership**:
|
||||
|
||||
| Pointer | Owner | C# obligation |
|
||||
|---|---|---|
|
||||
| Handler **input** (`params` string) | C++ — a stack object inside `RpcServer::process` | Copy the data out immediately. **Never** call `crpc_str_destroy` on it. |
|
||||
| Handler **return** value | Transfers to C++ — `rpc_server.cpp` destroys it after parsing | Create with `crpc_str_create`, return the handle, and **relinquish** C#-side ownership. |
|
||||
|
||||
Both rules are violated by the current `RpcTest.cs`:
|
||||
it `Dispose()`s the input handle (destroying an object it doesn't own) and
|
||||
returns a handle whose C# finalizer will later call `crpc_str_destroy` on
|
||||
memory C++ has already freed — a double-destroy race.
|
||||
|
||||
Spec: split the wrapper into two types.
|
||||
|
||||
```csharp
|
||||
/// Non-owning view over a C++-owned rpc_string. No finalizer, no Dispose.
|
||||
readonly ref struct BorrowedRpcString { /* Data property only */ }
|
||||
|
||||
/// C#-created rpc_string. Dispose() destroys it; Release() transfers
|
||||
/// ownership (nulls the handle and calls GC.SuppressFinalize).
|
||||
sealed class OwnedRpcString : IDisposable
|
||||
{
|
||||
public IntPtr Release() { var h = _handle; _handle = IntPtr.Zero;
|
||||
GC.SuppressFinalize(this); return h; }
|
||||
}
|
||||
```
|
||||
|
||||
Handlers end with `return new OwnedRpcString(json).Release();`.
|
||||
|
||||
## 3. Main-thread dispatcher
|
||||
|
||||
RPC callbacks arrive on a C++ per-client thread; Unity APIs are
|
||||
main-thread-only. The callback must block until the main thread produces the
|
||||
result. The `task.Wait()` idea in `RpcTest.cs` is directionally right but must
|
||||
be hardened (`Assets/Scripts/CloudPointRpc/MainThreadDispatcher.cs`):
|
||||
|
||||
- Per request, create
|
||||
`new TaskCompletionSource<string>(TaskCreationOptions.RunContinuationsAsynchronously)`
|
||||
and enqueue `(Func<string> handler, tcs)` into a `ConcurrentQueue`.
|
||||
- `Update()` drains the queue: run the handler, `tcs.TrySetResult(json)`
|
||||
(or `TrySetException`).
|
||||
- The callback thread waits with a **bounded timeout**:
|
||||
`tcs.Task.Wait(TimeoutMs)` with ~5000 ms. On timeout, return a JSON string
|
||||
with an `"error"` field (the C++ side passes the string through as the
|
||||
result). An unbounded wait deadlocks permanently when the editor is paused
|
||||
or the component is destroyed.
|
||||
- Note that `server_api.cpp` holds a global `server_mtx` while dispatching, so
|
||||
**one stuck handler stalls every client** — another reason the timeout is
|
||||
mandatory.
|
||||
|
||||
### Shutdown ordering (deadlock hazard)
|
||||
|
||||
`crpc_deinit()` joins server threads. A server thread may be blocked inside a
|
||||
callback waiting for the main thread — which is the thread calling
|
||||
`crpc_deinit()`. Required order in `OnDestroy`:
|
||||
|
||||
1. Set a `volatile bool _shuttingDown` — new callbacks fail-fast with an error
|
||||
result; pending queue entries get `TrySetCanceled()`.
|
||||
2. Drain/clear the queue.
|
||||
3. Only then call `crpc_deinit()`.
|
||||
|
||||
## 4. Required handlers
|
||||
|
||||
### `get-stereo-calibration`
|
||||
|
||||
Derive intrinsics from the rendering camera, extrinsics from the two camera
|
||||
transforms (schema in API.md):
|
||||
|
||||
- `fx = width / (2 * tan(hFov / 2))`, `fy = height / (2 * tan(vFov / 2))`
|
||||
where `vFov = Camera.fieldOfView` (degrees → radians) and
|
||||
`hFov = 2 * atan(tan(vFov/2) * aspect)`. Use the render-texture resolution,
|
||||
not the screen resolution.
|
||||
- `cx = width / 2`, `cy = height / 2`, skew 0. `dist_coeffs` = five zeros
|
||||
(ideal pinhole).
|
||||
- Extrinsics: right camera relative to left in OpenCV convention
|
||||
(`x_r = R·x_l + T`), converted per §5. For the standard parallel rig this is
|
||||
`R = I`, `T = [-baseline, 0, 0]` with
|
||||
`baseline = Vector3.Distance(left.position, right.position)` in meters.
|
||||
- `image_size` = the RenderTexture size served by `get-image-pair`.
|
||||
|
||||
### `get-image-pair`
|
||||
|
||||
Both eyes must be captured on the **same rendered frame**:
|
||||
|
||||
- Give both cameras `targetTexture` RenderTextures; inside the (main-thread)
|
||||
handler call `leftCam.Render(); rightCam.Render();` then read both back.
|
||||
- Readback: synchronous `Texture2D.ReadPixels` inside the handler is
|
||||
acceptable and simplest — the RPC thread is blocked waiting anyway.
|
||||
`AsyncGPUReadback` is the documented optimization: issue the request in the
|
||||
handler and complete the `TaskCompletionSource` from the readback callback
|
||||
(the dispatcher design above already supports deferred completion).
|
||||
- **Vertical flip**: GPU readbacks are bottom-up; the wire format is
|
||||
top-left-origin row-major (API.md). Flip rows before encoding.
|
||||
- Encode with `Convert.ToBase64String`; fill the `get-image-pair` schema with
|
||||
`type: "RGBA"` (or convert to BGR to save 25% payload).
|
||||
- Include a monotonically increasing `frame` counter
|
||||
(`Time.frameCount` is fine).
|
||||
|
||||
### Legacy methods
|
||||
|
||||
`get-intrinsic-params` / `get-extrinsic-params` may be kept, backed by the
|
||||
left camera, for backward compatibility. They are not used by
|
||||
`CloudPointClient`.
|
||||
|
||||
## 5. Coordinate conventions
|
||||
|
||||
- Unity: left-handed, +Y up, +Z forward. OpenCV camera frame: right-handed,
|
||||
+X right, **+Y down**, +Z forward.
|
||||
- Conversion with `S = diag(1, -1, 1)`:
|
||||
`R_cv = S · R_unity · S`, `t_cv = S · t_unity`.
|
||||
- The reconstructed cloud is in the **OpenCV left-camera frame**. Consumers
|
||||
that need Unity world space must apply the inverse conversion plus the left
|
||||
camera pose; if needed, add an optional `left_pose` (16 doubles, row-major
|
||||
4x4) field to `get-stereo-calibration`.
|
||||
- All matrices row-major flattened; all lengths in meters (1 Unity unit = 1 m);
|
||||
disparity in pixels.
|
||||
|
||||
## 6. Performance notes
|
||||
|
||||
- 1920x1080 RGBA is 8.3 MB raw ≈ 11 MB base64 per eye, ~22 MB per
|
||||
`get-image-pair` — of the order 100–300 ms per fetch on loopback. For
|
||||
interactive rates use 960x540 and/or BGR.
|
||||
- `ReadPixels` stalls the GPU pipeline (~1–5 ms at 1080p): fine for on-demand
|
||||
capture; do not capture every frame unconditionally.
|
||||
- Base64 of large buffers allocates heavily; reuse buffers
|
||||
(`Convert.TryToBase64Chars`) where practical.
|
||||
- The server serializes requests globally (`server_mtx`), so handlers need no
|
||||
reentrancy protection — but a slow handler blocks all clients (see §3).
|
||||
|
||||
## 7. Verification checklist
|
||||
|
||||
1. Start the Unity scene (server on the configured port).
|
||||
2. From this repo: `./build/src/cloud_point_rpc_cli config.yaml`, option
|
||||
`4` (compute-cloud) — expect a plausible point count and bounding box.
|
||||
3. Option `5` writes a PLY; inspect it in MeshLab against the visible scene
|
||||
geometry (a plane at 1.5 m should reconstruct at z ≈ 1.5).
|
||||
4. Kill the client mid-request and re-connect — the server must keep serving
|
||||
(per-client threads are independent; the TCP loop tolerates EOF).
|
||||
76
include/cloud_point/cloud_point_client.hpp
Normal file
76
include/cloud_point/cloud_point_client.hpp
Normal file
@ -0,0 +1,76 @@
|
||||
#pragma once
|
||||
|
||||
#include "cloud_point/point_cloud_builder.hpp"
|
||||
#include "cloud_point/stereo_matcher.hpp"
|
||||
#include "cloud_point/stereo_matcher_factory.hpp"
|
||||
#include "cloud_point/stereo_rectifier.hpp"
|
||||
#include <expected>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
namespace score {
|
||||
|
||||
class TCPConnector;
|
||||
class RpcClient;
|
||||
|
||||
/// @brief End-to-end stereo point cloud client.
|
||||
///
|
||||
/// Connects to a JSON-RPC stereo server, fetches calibration once on connect(),
|
||||
/// then produces point clouds on demand via compute_cloud().
|
||||
///
|
||||
/// Thread safety: not thread-safe. Do not call methods concurrently.
|
||||
class CloudPointClient {
|
||||
public:
|
||||
/// @brief Recoverable per-frame error.
|
||||
struct Error {
|
||||
std::string message;
|
||||
};
|
||||
|
||||
/// @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
|
||||
/// unavailable).
|
||||
/// @param opts Depth filtering options.
|
||||
/// @param num_disparities SGBM disparity levels (default 128; use 160 for
|
||||
/// small-baseline rigs such as SCARED).
|
||||
CloudPointClient(std::string ip, int port,
|
||||
StereoAlgorithmType algo = StereoAlgorithmType::GPU,
|
||||
PointCloudBuilder::Options opts = {},
|
||||
int num_disparities = 128);
|
||||
|
||||
~CloudPointClient();
|
||||
|
||||
/// @brief Connect to server and fetch calibration once.
|
||||
/// @throws std::runtime_error on connection or calibration failure.
|
||||
void connect();
|
||||
|
||||
/// @brief Return true if connected and all pipeline components are ready.
|
||||
[[nodiscard]] bool connected() const noexcept;
|
||||
|
||||
/// @brief Compute one point cloud: fetch image pair → gray → rectify →
|
||||
/// disparity → reproject.
|
||||
///
|
||||
/// Recoverable per-frame failures (RPC error, decode error) are returned as
|
||||
/// Error. Calling before connect() returns Error immediately.
|
||||
[[nodiscard]] std::expected<PointCloud, Error> compute_cloud();
|
||||
|
||||
private:
|
||||
std::string ip_;
|
||||
int port_;
|
||||
StereoAlgorithmType algo_;
|
||||
PointCloudBuilder::Options opts_;
|
||||
int num_disparities_;
|
||||
std::unique_ptr<TCPConnector> connector_;
|
||||
std::unique_ptr<RpcClient> client_;
|
||||
std::unique_ptr<StereoRectifier> rectifier_;
|
||||
std::unique_ptr<IStereoMatcher> matcher_;
|
||||
std::unique_ptr<PointCloudBuilder> builder_;
|
||||
};
|
||||
|
||||
/// @brief Write valid points as ASCII PLY (for MeshLab inspection).
|
||||
/// @param cloud Source point cloud.
|
||||
/// @param path Output file path.
|
||||
void write_ply(const PointCloud &cloud, const std::string &path);
|
||||
|
||||
} // namespace score
|
||||
@ -8,7 +8,7 @@ namespace score {
|
||||
/// @brief CPU-based stereo matcher using cv::StereoSGBM.
|
||||
class CpuStereoMatcher : public IStereoMatcher {
|
||||
public:
|
||||
CpuStereoMatcher(int min_disparity = 0, int num_disparities = 16,
|
||||
CpuStereoMatcher(int min_disparity = 0, int num_disparities = 128,
|
||||
int block_size = 3);
|
||||
~CpuStereoMatcher() override = default;
|
||||
|
||||
|
||||
66
include/cloud_point/point_cloud_builder.hpp
Normal file
66
include/cloud_point/point_cloud_builder.hpp
Normal file
@ -0,0 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <limits>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace score {
|
||||
|
||||
/// @brief Dense point cloud in row-major XYZ layout.
|
||||
///
|
||||
/// Invalid (occluded or out-of-range) points are represented as quiet NaN on
|
||||
/// all three coordinates. valid_points() filters them out.
|
||||
struct PointCloud {
|
||||
int width{0};
|
||||
int height{0};
|
||||
std::vector<float> data; ///< width * height * 3 floats, XYZ row-major
|
||||
|
||||
/// @brief Return only the non-NaN points.
|
||||
[[nodiscard]] std::vector<std::array<float, 3>> valid_points() const;
|
||||
};
|
||||
|
||||
/// @brief Reprojects a disparity map to a 3-D point cloud using the
|
||||
/// reprojection matrix Q produced by cv::stereoRectify.
|
||||
///
|
||||
/// Thread safety: build() is const and safe to call concurrently once the
|
||||
/// object is constructed.
|
||||
class PointCloudBuilder {
|
||||
public:
|
||||
/// @brief Depth filtering thresholds.
|
||||
struct Options {
|
||||
float min_depth_m;
|
||||
float max_depth_m;
|
||||
// Explicit constructor avoids a GCC limitation with nested-struct
|
||||
// default-member-initialisers used as default function arguments.
|
||||
Options() noexcept : min_depth_m(0.01f), max_depth_m(10.0f) {}
|
||||
Options(float min_m, float max_m) noexcept
|
||||
: min_depth_m(min_m), max_depth_m(max_m) {}
|
||||
};
|
||||
|
||||
/// @brief Construct builder.
|
||||
/// @param q 4x4 CV_64F reprojection matrix from cv::stereoRectify.
|
||||
/// @param opts Optional depth-range filter.
|
||||
/// @throws std::invalid_argument if q is not 4x4 CV_64F.
|
||||
explicit PointCloudBuilder(cv::Mat q, Options opts = Options{});
|
||||
|
||||
/// @brief Reproject disparity to a point cloud.
|
||||
///
|
||||
/// Accepts:
|
||||
/// - CV_16S SGBM fixed-point output (values = disparity * 16)
|
||||
/// - CV_32F already in pixel units
|
||||
///
|
||||
/// Invalid points (disparity ≤ 0, |z| ≥ 10000, or z outside
|
||||
/// [min_depth_m, max_depth_m]) are stored as quiet NaN.
|
||||
///
|
||||
/// @param disparity Disparity map (CV_16S or CV_32F).
|
||||
/// @return Populated PointCloud.
|
||||
/// @throws std::invalid_argument on unsupported disparity type.
|
||||
[[nodiscard]] PointCloud build(const cv::Mat &disparity) const;
|
||||
|
||||
private:
|
||||
cv::Mat q_;
|
||||
Options opts_;
|
||||
};
|
||||
|
||||
} // namespace score
|
||||
@ -1,16 +0,0 @@
|
||||
#pragma once
|
||||
#include "cloud_point/image.h"
|
||||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
namespace score {
|
||||
|
||||
class Rectify {
|
||||
public:
|
||||
Rectify();
|
||||
~Rectify();
|
||||
|
||||
void perform(Image &image, cv::Mat cameraMatrix,
|
||||
cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F));
|
||||
};
|
||||
|
||||
} // namespace score
|
||||
@ -12,8 +12,9 @@ class StereoMatcherFactory {
|
||||
public:
|
||||
/// @brief Create a stereo matcher of the requested type.
|
||||
/// If GPU is requested but unavailable, falls back to CPU.
|
||||
/// @param num_disparities Number of disparity levels for SGBM (default 128).
|
||||
[[nodiscard]] static std::unique_ptr<IStereoMatcher>
|
||||
create(StereoAlgorithmType type);
|
||||
create(StereoAlgorithmType type, int num_disparities = 128);
|
||||
};
|
||||
|
||||
} // namespace score
|
||||
|
||||
60
include/cloud_point/stereo_rectifier.hpp
Normal file
60
include/cloud_point/stereo_rectifier.hpp
Normal file
@ -0,0 +1,60 @@
|
||||
#pragma once
|
||||
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "cloud_point_rpc/rpc_dto.hpp"
|
||||
|
||||
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.
|
||||
class StereoRectifier {
|
||||
public:
|
||||
/// @brief Calibration parameters for a stereo rig.
|
||||
struct Calibration {
|
||||
cv::Mat k_left; ///< 3x3 CV_64F intrinsic matrix, left camera
|
||||
cv::Mat d_left; ///< 1x5 CV_64F distortion coefficients, left camera
|
||||
cv::Mat k_right; ///< 3x3 CV_64F intrinsic matrix, right camera
|
||||
cv::Mat d_right; ///< 1x5 CV_64F distortion coefficients, right camera
|
||||
cv::Mat r; ///< 3x3 CV_64F rotation from left to right camera
|
||||
cv::Mat
|
||||
t; ///< 3x1 CV_64F translation from left to right camera (metres)
|
||||
cv::Size image_size;
|
||||
|
||||
/// @brief Build calibration from the wire DTO using
|
||||
/// CameraMatrixFactory.
|
||||
/// @param rpc Stereo calibration received over JSON-RPC.
|
||||
/// @return Populated Calibration struct.
|
||||
static Calibration from_rpc(const StereoCalibrationRPC &rpc);
|
||||
};
|
||||
|
||||
/// @brief Construct rectifier from calibration data.
|
||||
///
|
||||
/// Calls cv::stereoRectify (CALIB_ZERO_DISPARITY, alpha=0) and
|
||||
/// cv::initUndistortRectifyMap (CV_16SC2) for both sides.
|
||||
///
|
||||
/// @throws std::invalid_argument if any Mat has the wrong size or type.
|
||||
explicit StereoRectifier(const Calibration &calib);
|
||||
|
||||
/// @brief Apply rectification maps to a stereo pair.
|
||||
/// @param left Left input image (any type accepted by cv::remap).
|
||||
/// @param right Right input image.
|
||||
/// @return {rectified_left, rectified_right}.
|
||||
[[nodiscard]] std::pair<cv::Mat, cv::Mat>
|
||||
rectify(const cv::Mat &left, const cv::Mat &right) 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 q_; ///< 4x4 CV_64F reprojection matrix
|
||||
};
|
||||
|
||||
} // namespace score
|
||||
@ -132,19 +132,21 @@ class CRPC_EXPORT TcpServer {
|
||||
private:
|
||||
void handle_client(std::shared_ptr<asio::ip::tcp::socket> socket) {
|
||||
LOG(INFO) << "Server reading from client...";
|
||||
try {
|
||||
auto payload = tcp_read(*socket, "TCPServer] ");
|
||||
size_t payload_length = payload.size();
|
||||
if (payload_length > 0) {
|
||||
while (true) {
|
||||
try {
|
||||
auto payload = tcp_read(*socket, "TCPServer] ");
|
||||
if (payload.empty())
|
||||
break; // connection closed or read error
|
||||
std::string response = processor_(payload);
|
||||
response += "\n";
|
||||
DLOG(INFO) << "Server sending response: " << response;
|
||||
inplace_size_embedding(response);
|
||||
asio::write(*socket, asio::buffer(response));
|
||||
LOG(INFO) << "Server sent response";
|
||||
} catch (const std::exception &e) {
|
||||
LOG(WARNING) << "Client handling error: " << e.what();
|
||||
break;
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
LOG(WARNING) << "Client handling error: " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
project('cloud_point_rpc', 'cpp',
|
||||
version : '0.1',
|
||||
default_options : ['warning_level=3', 'cpp_std=c++20'])
|
||||
default_options : ['warning_level=3', 'cpp_std=c++23'])
|
||||
|
||||
# Dependencies
|
||||
json_dep = dependency('nlohmann_json', fallback : ['nlohmann_json', 'nlohmann_json_dep'])
|
||||
|
||||
@ -9,16 +9,23 @@ The system follows a layered architecture for JSON-RPC 2.0 communication over TC
|
||||
│ Application / Unity │
|
||||
│ (registers callbacks via C API or server_main.cpp) │
|
||||
├──────────────────────────────────────────────────────┤
|
||||
│ OpenCV Compute Layer (optional — requires opencv4) │
|
||||
│ CloudPointClient (facade: connect, compute_cloud, │
|
||||
│ write_ply) │
|
||||
│ StereoRectifier (cv::stereoRectify + remap, Q mat) │
|
||||
│ PointCloudBuilder (SGBM disparity, reproject, NaN │
|
||||
│ filter) │
|
||||
├──────────────────────────────────────────────────────┤
|
||||
│ RPC Layer │
|
||||
│ RpcServer (method dispatch, JSON-RPC 2.0 handling) │
|
||||
│ RpcClient (typed method calls via jsonrpccxx) │
|
||||
├──────────────────────────────────────────────────────┤
|
||||
│ Service Layer │
|
||||
│ Service (mock camera data: intrinsics, extrinsics, │
|
||||
│ cloud points) │
|
||||
│ stereo calibration, image pairs) │
|
||||
├──────────────────────────────────────────────────────┤
|
||||
│ Transport Layer │
|
||||
│ TcpServer (accept loop, per-client threads) │
|
||||
│ TcpServer (accept loop, per-client threads — loops) │
|
||||
│ TCPConnector (client-side connector for jsonrpccxx) │
|
||||
│ tcp_read (framed read with size prefix) │
|
||||
├──────────────────────────────────────────────────────┤
|
||||
@ -37,14 +44,14 @@ All C++ code lives in the `score` namespace. The `rpc/` git submodule provides `
|
||||
|
||||
The PlantUML diagram at `docs/communication_model.pu` (rendered as `docs/cm.png`) describes the interaction flow:
|
||||
|
||||
1. **Unity side** initializes a `CloudPointServer` which starts a `TcpServer` awaiting connections.
|
||||
2. **Client side** initializes a `CloudPointClient` which creates a `TCPConnector` and connects to the server.
|
||||
3. When a **Caller** requests data, the client serializes a JSON-RPC request, sends it over TCP, and waits for the response.
|
||||
4. The server reads the request, dispatches it to `RpcServer`, which calls the registered handler (either a C++ lambda or a C callback from Unity).
|
||||
5. The handler result is serialized back as a JSON-RPC response and sent over TCP.
|
||||
6. The client receives the response and returns typed data to the caller.
|
||||
1. **Unity side** initializes a `CloudPointServer` via `crpc_init()`, which starts a `TcpServer` awaiting connections.
|
||||
2. **Client side** calls `CloudPointClient::connect()`, which opens a TCP connection and immediately fetches `get-stereo-calibration` once, initialising `StereoRectifier` with the returned intrinsics and geometry.
|
||||
3. On each `compute_cloud()` call, the client fetches a synchronised image pair via `get-image-pair`, passes the decoded images through `StereoRectifier::rectify()`, then `PointCloudBuilder::build()` (SGBM disparity at 1/16 scale → `cv::reprojectImageTo3D` → NaN filter), and returns `std::expected<PointCloud, Error>` to the caller.
|
||||
4. The server reads each request, dispatches it to `RpcServer`, which calls the registered handler (a C callback from Unity).
|
||||
5. The Unity integration uses a **main-thread dispatcher**: RPC callbacks arrive on a C++ per-client thread and enqueue a `TaskCompletionSource` onto a `ConcurrentQueue`; the Unity `Update()` loop drains the queue and completes the task. A bounded timeout (~5000 ms) prevents deadlocks when the editor is paused.
|
||||
6. The handler result is serialized back as a JSON-RPC response and sent over TCP; the per-client handler thread loops back to await the next request on the same connection.
|
||||
|
||||
The Unity integration model uses a **static queue**: the server enqueues tasks, and the Unity main loop dequeues and executes them, then sets the return value. This avoids calling Unity APIs from non-main threads.
|
||||
See [docs/unity-integration.md](../docs/unity-integration.md) for the full Unity C# design spec.
|
||||
|
||||
## Wire framing
|
||||
|
||||
@ -153,6 +160,6 @@ Non-object JSON requests (arrays, strings, numbers, null) throw `nlohmann::json:
|
||||
|
||||
## Threading model
|
||||
|
||||
- **Server**: Accept thread + per-client handler threads (all `std::jthread`). Each client handler reads one request, processes it, sends the response, and exits. The server does not keep connections open for multiple round-trips per thread.
|
||||
- **Server**: Accept thread + per-client handler threads (all `std::jthread`). Each client handler loops: it reads one request, processes it, sends the response, and then loops back to read the next request on the same connection. The connection remains open until the client disconnects (EOF) or an error occurs.
|
||||
- **C API server** (`server_api.cpp`): The global `RpcServer` and `TcpServer` are guarded by `server_mtx`. The `rpc_string` garbage collector is guarded by `gc_mtx`.
|
||||
- **Test API** (`test_api.cpp`): A `TestThread` runs a `std::jthread` with a condition-variable-driven loop that can auto-call registered methods at a configurable interval or process queued one-shot calls.
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
## Build system
|
||||
|
||||
The project uses **Meson** with **Ninja** and requires a **C++20** compiler (GCC or Clang). The root `meson.build` declares the project and dependencies; `src/meson.build` and `tests/meson.build` define build targets.
|
||||
The project uses **Meson** with **Ninja** and requires a **C++23** compiler (GCC or Clang). The root `meson.build` declares the project and dependencies; `src/meson.build` and `tests/meson.build` define build targets.
|
||||
|
||||
### Dependencies
|
||||
|
||||
|
||||
@ -2,6 +2,8 @@
|
||||
|
||||
The C API allows Unity (or any C/C++ consumer) to embed the RPC server as a shared library, register custom RPC handlers as C function pointers, and manage the server lifecycle without touching C++ directly.
|
||||
|
||||
> **Unity C# design spec**: For the complete Unity-side implementation guide — including `rpc_string` ownership rules, IL2CPP callback pinning, main-thread dispatcher design, and shutdown ordering — see [docs/unity-integration.md](../docs/unity-integration.md). The C API described here is the C++ side of the interface; the C# side is the Unity project's responsibility.
|
||||
|
||||
Two C API surfaces exist:
|
||||
|
||||
| API | Header | Library | Purpose |
|
||||
|
||||
@ -2,9 +2,9 @@
|
||||
|
||||
## What is this?
|
||||
|
||||
Cloud Point RPC is a **C++20 JSON-RPC 2.0** server and client implementation designed to bridge a C++ backend with a **Unity Scene** over TCP. The server exposes RPC methods that retrieve camera intrinsic/extrinsic parameters and point cloud data. A C API (`server_api.h`) allows Unity (or other C consumers) to embed the server, register custom RPC handlers, and manage the server lifecycle from native code.
|
||||
Cloud Point RPC is a **C++23 JSON-RPC 2.0** server and client implementation designed to bridge a C++ backend with a **Unity Scene** over TCP. Unity serves stereo camera data (`get-stereo-calibration`, `get-image-pair`) over the embedded RPC server; the C++ `CloudPointClient` fetches calibration once on `connect()`, then on each `compute_cloud()` call retrieves a synchronised image pair, runs stereo rectification + SGBM disparity + `cv::reprojectImageTo3D`, and returns a filtered `PointCloud`. A C API (`server_api.h`) allows Unity to embed the server, register custom RPC handlers, and manage the server lifecycle from native code.
|
||||
|
||||
The project is a work in progress: the server side with C-API is implemented, while the client side with OpenCV integration is still planned (see README TODO).
|
||||
The server side with C-API is fully implemented. The C++ stereo point-cloud client (`CloudPointClient`, `StereoRectifier`, `PointCloudBuilder`) is implemented. The remaining item is the Unity-side C# implementation; see [docs/unity-integration.md](../docs/unity-integration.md).
|
||||
|
||||
## Repository layout
|
||||
|
||||
@ -18,7 +18,9 @@ The project is a work in progress: the server side with C-API is implemented, wh
|
||||
| `tests/` | GTest/GMock unit and integration tests (single `unit_tests` executable) |
|
||||
| `rpc/` | Git submodule — [json-rpc-cxx](https://github.com/jsonrpcx/json-rpc-cxx) providing `jsonrpccxx` headers |
|
||||
| `subprojects/` | Meson wrap dependencies (asio, nlohmann_json, glog, yaml-cpp, base64, gtest) |
|
||||
| `docs/` | PlantUML communication model diagram |
|
||||
| `include/cloud_point/` | OpenCV compute library headers: `StereoRectifier`, `PointCloudBuilder`, `CloudPointClient` |
|
||||
| `src/cloud_point/` | OpenCV compute library implementation (optional, requires opencv4) |
|
||||
| `docs/` | PlantUML communication model diagram and Unity integration design spec |
|
||||
| `config.yml` | Sample server configuration (IP and port) |
|
||||
| `Dockerfile` | Container image for the CLI client |
|
||||
| `.gitea/workflows/test.yaml` | CI pipeline (build + test on push to master) |
|
||||
@ -43,6 +45,13 @@ Run the interactive CLI client:
|
||||
./build/src/cloud_point_rpc_cli config.yaml
|
||||
```
|
||||
|
||||
CLI menu options (options 4 and 5 are hidden when built without opencv4):
|
||||
|
||||
| Option | Action |
|
||||
|--------|--------|
|
||||
| 4 | Compute point cloud — prints point count and bounding box |
|
||||
| 5 | Compute point cloud and save to `output.ply` |
|
||||
|
||||
Run all tests:
|
||||
|
||||
```bash
|
||||
@ -63,4 +72,6 @@ For Windows build instructions and Docker usage, see [Build & Testing](build-and
|
||||
- **Namespace**: All C++ code lives in `score` (renamed from `cloud_point_rpc` early in development).
|
||||
- **Wire framing**: Every TCP message is prefixed with an 8-byte little-endian `uint64_t` payload size, then the JSON-RPC payload follows. See [Architecture → Wire framing](architecture.md#wire-framing).
|
||||
- **Two server entrypoints**: `server_main.cpp` is a standalone executable with mock data; `server_api.cpp` provides the embeddable C API that Unity uses to start the server and register callbacks.
|
||||
- **Base64**: Camera parameter arrays and point cloud data are Base64-encoded for ASCII-safe transport over JSON. Encoding/decoding is done on the Unity side per API.md.
|
||||
- **Base64**: Image pixel payloads (`get-image-pair`) are Base64-encoded for ASCII-safe transport over JSON. Calibration arrays are plain JSON doubles — not Base64. Encoding on the Unity side; decoding in `Base64RPCCoder` on the client side per API.md.
|
||||
- **Persistent connections**: `TcpServer::handle_client` loops per connection — multiple RPC round-trips share one TCP connection without reconnecting.
|
||||
- **std::expected**: The `cloud_point` compute library uses `std::expected<PointCloud, Error>` (C++23) as its return type. Callers check `has_value()` before accessing the result.
|
||||
@ -57,37 +57,87 @@ Source: `src/rpc_server.cpp` — `RpcServer::process()` and `create_error()`.
|
||||
|
||||
## Methods
|
||||
|
||||
### `get-intrinsic-params`
|
||||
### `get-stereo-calibration`
|
||||
|
||||
Retrieves intrinsic camera parameters as a flat 3×3 matrix (row-major, 9 doubles).
|
||||
Returns full stereo rig calibration: intrinsics for both cameras, stereo rotation and translation, and sensor resolution. Called once by `CloudPointClient::connect()`.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "method": "get-intrinsic-params", "id": 1}
|
||||
{"jsonrpc": "2.0", "method": "get-stereo-calibration", "id": 1}
|
||||
```
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "result": <base64-encoded-array>, "id": 1}
|
||||
{
|
||||
"left": { "camera_matrix": [9 doubles], "dist_coeffs": [5 doubles] },
|
||||
"right": { "camera_matrix": [9 doubles], "dist_coeffs": [5 doubles] },
|
||||
"rotation": [9 doubles],
|
||||
"translation": [3 doubles],
|
||||
"image_size": { "width": 640, "height": 480 }
|
||||
}
|
||||
```
|
||||
|
||||
Result type: `vector<double>` (size 9), Base64-encoded.
|
||||
All matrices are row-major. `camera_matrix` is the 3×3 intrinsic matrix; `rotation` is the 3×3 left-to-right rotation (OpenCV convention); `translation` is in metres. Calibration arrays are **plain JSON doubles — not Base64**.
|
||||
|
||||
### `get-extrinsic-params`
|
||||
Test mock defaults: fx=fy=800, cx=320, cy=240, zero distortion, R=I, T=[-0.06, 0, 0], 640×480.
|
||||
|
||||
Retrieves extrinsic camera parameters as a flat 4×4 matrix (row-major, 16 doubles).
|
||||
---
|
||||
|
||||
### `get-image-pair`
|
||||
|
||||
Returns a synchronised stereo frame as two base64-encoded images. Called on each `CloudPointClient::compute_cloud()`.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "method": "get-extrinsic-params", "id": 2}
|
||||
{"jsonrpc": "2.0", "method": "get-image-pair", "id": 2}
|
||||
```
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "result": <base64-encoded-array>, "id": 2}
|
||||
{
|
||||
"frame": 42,
|
||||
"left": { "width": 640, "height": 480, "type": "BGR", "data": "<base64>" },
|
||||
"right": { "width": 640, "height": 480, "type": "BGR", "data": "<base64>" }
|
||||
}
|
||||
```
|
||||
|
||||
Result type: `vector<double>` (size 16), Base64-encoded.
|
||||
`data` is Base64-encoded raw pixel bytes (standard alphabet, no line breaks). `type` is `"BGR"` (3 ch), `"RGBA"` (4 ch), or `"DEPTH"` (1 ch float32). Unity must flip GPU readback vertically before encoding (wire format is top-left-origin row-major).
|
||||
|
||||
DTOs: `StereoCalibrationDto` and `ImagePairDto` in `include/cloud_point_rpc/rpc_dto.hpp`. Decoded client-side via `Base64RPCCoder`.
|
||||
|
||||
---
|
||||
|
||||
### `get-intrinsic-params` *(legacy)*
|
||||
|
||||
Retrieves left-camera intrinsic parameters as a flat 3×3 matrix (row-major, 9 doubles). Not used by `CloudPointClient`; kept for backward compatibility.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "method": "get-intrinsic-params", "id": 3}
|
||||
```
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "result": [fx, 0, cx, 0, fy, cy, 0, 0, 1], "id": 3}
|
||||
```
|
||||
|
||||
Result: 9 plain JSON doubles. **Not Base64.**
|
||||
|
||||
### `get-extrinsic-params` *(legacy)*
|
||||
|
||||
Retrieves left-camera extrinsic matrix as a flat 4×4 matrix (row-major, 16 doubles). Not used by `CloudPointClient`; kept for backward compatibility.
|
||||
|
||||
**Request:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "method": "get-extrinsic-params", "id": 4}
|
||||
```
|
||||
|
||||
**Response:**
|
||||
```json
|
||||
{"jsonrpc": "2.0", "result": [16 doubles], "id": 4}
|
||||
```
|
||||
|
||||
Result: 16 plain JSON doubles. **Not Base64.**
|
||||
|
||||
### `get-cloud-point`
|
||||
|
||||
@ -146,7 +196,7 @@ Source: `src/rpc_server.cpp` — `register_method(name, callback_t)` overload.
|
||||
- `encode(vector<char>) → string` and `decode(string) → vector<char>`.
|
||||
- Includes overflow protection: throws `std::length_error` if input exceeds safe size limits.
|
||||
|
||||
> Per `API.md`, Base64 encoding/decoding of camera data is expected to be done on the **Unity side**. The `Base64RPCCoder` class is available for C++ consumers but is not used by the mock server's response path.
|
||||
> Per `API.md`: **only image pixel data** (`get-image-pair` `data` fields) uses Base64. Calibration arrays (`camera_matrix`, `dist_coeffs`, `rotation`, `translation`) and legacy params are plain JSON doubles — never Base64. Unity encodes images before sending; the C++ client decodes them via `Base64RPCCoder`.
|
||||
|
||||
## Client-side usage
|
||||
|
||||
|
||||
117
src/cli.cpp
117
src/cli.cpp
@ -3,6 +3,13 @@
|
||||
#include <glog/logging.h>
|
||||
#include <string>
|
||||
|
||||
#ifdef HAVE_CLOUD_POINT_COMPUTE
|
||||
#include "cloud_point/cloud_point_client.hpp"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#endif
|
||||
|
||||
namespace score {
|
||||
|
||||
void print_menu(std::ostream &output) {
|
||||
@ -10,6 +17,8 @@ void print_menu(std::ostream &output) {
|
||||
output << "1. get-intrinsic-params" << std::endl;
|
||||
output << "2. get-extrinsic-params" << std::endl;
|
||||
output << "3. get-cloud-point" << std::endl;
|
||||
output << "4. compute-cloud" << std::endl;
|
||||
output << "5. compute-cloud + save PLY" << std::endl;
|
||||
output << "0. Exit" << std::endl;
|
||||
output << "Select an option: ";
|
||||
}
|
||||
@ -48,34 +57,90 @@ int run_cli(std::istream &input, std::ostream &output, const std::string &ip,
|
||||
if (choice == "0")
|
||||
break;
|
||||
|
||||
std::string method;
|
||||
if (choice == "1") {
|
||||
method = "get-intrinsic-params";
|
||||
} else if (choice == "2") {
|
||||
method = "get-extrinsic-params";
|
||||
} else if (choice == "3") {
|
||||
method = "get-cloud-point";
|
||||
if (choice == "1" || choice == "2" || choice == "3") {
|
||||
std::string method;
|
||||
if (choice == "1") {
|
||||
method = "get-intrinsic-params";
|
||||
} else if (choice == "2") {
|
||||
method = "get-extrinsic-params";
|
||||
} else {
|
||||
method = "get-cloud-point";
|
||||
}
|
||||
|
||||
try {
|
||||
if (method == "get-intrinsic-params") {
|
||||
auto response = client.get_intrinsic_params();
|
||||
output << vector_to_string(response);
|
||||
}
|
||||
if (method == "get-extrinsic-params") {
|
||||
auto response = client.get_extrinsic_params();
|
||||
output << vector_to_string(response);
|
||||
}
|
||||
if (method == "get-cloud-point") {
|
||||
auto response = client.get_cloud_point();
|
||||
output << vector_to_string(response);
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
output << "\nRPC Error: " << e.what() << std::endl;
|
||||
}
|
||||
} else if (choice == "4" || choice == "5") {
|
||||
#ifdef HAVE_CLOUD_POINT_COMPUTE
|
||||
try {
|
||||
CloudPointClient cpc(ip, port, StereoAlgorithmType::GPU);
|
||||
cpc.connect();
|
||||
auto result = cpc.compute_cloud();
|
||||
|
||||
if (!result) {
|
||||
output << "Error: " << result.error().message
|
||||
<< std::endl;
|
||||
} else {
|
||||
const auto &cloud = *result;
|
||||
const auto valid = cloud.valid_points();
|
||||
|
||||
float x_min = std::numeric_limits<float>::max();
|
||||
float x_max = std::numeric_limits<float>::lowest();
|
||||
float y_min = std::numeric_limits<float>::max();
|
||||
float y_max = std::numeric_limits<float>::lowest();
|
||||
float z_min = std::numeric_limits<float>::max();
|
||||
float z_max = std::numeric_limits<float>::lowest();
|
||||
|
||||
for (const auto &pt : valid) {
|
||||
x_min = std::min(x_min, pt[0]);
|
||||
x_max = std::max(x_max, pt[0]);
|
||||
y_min = std::min(y_min, pt[1]);
|
||||
y_max = std::max(y_max, pt[1]);
|
||||
z_min = std::min(z_min, pt[2]);
|
||||
z_max = std::max(z_max, pt[2]);
|
||||
}
|
||||
|
||||
output << "Cloud: " << cloud.width << "x"
|
||||
<< cloud.height << " valid_pts=" << valid.size()
|
||||
<< "\n";
|
||||
if (!valid.empty()) {
|
||||
output << " bbox x=[" << x_min << "," << x_max
|
||||
<< "]"
|
||||
<< " y=[" << y_min << "," << y_max << "]"
|
||||
<< " z=[" << z_min << "," << z_max << "]\n";
|
||||
}
|
||||
|
||||
if (choice == "5") {
|
||||
output << "PLY output path: ";
|
||||
std::string path;
|
||||
if (input >> path) {
|
||||
write_ply(cloud, path);
|
||||
output << "Saved " << valid.size()
|
||||
<< " points to " << path << "\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
output << "Error: " << e.what() << std::endl;
|
||||
}
|
||||
#else
|
||||
output << "OpenCV support not built" << std::endl;
|
||||
#endif
|
||||
} else {
|
||||
output << "Invalid option: " << choice << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
try {
|
||||
|
||||
if (method == "get-intrinsic-params") {
|
||||
auto response = client.get_intrinsic_params();
|
||||
output << vector_to_string(response);
|
||||
}
|
||||
if (method == "get-extrinsic-params") {
|
||||
auto response = client.get_extrinsic_params();
|
||||
output << vector_to_string(response);
|
||||
}
|
||||
if (method == "get-cloud-point") {
|
||||
auto response = client.get_cloud_point();
|
||||
output << vector_to_string(response);
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
output << "\nRPC Error: " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
|
||||
115
src/cloud_point/cloud_point_client.cpp
Normal file
115
src/cloud_point/cloud_point_client.cpp
Normal file
@ -0,0 +1,115 @@
|
||||
#include "cloud_point/cloud_point_client.hpp"
|
||||
|
||||
#include "cloud_point/imageFactory.h"
|
||||
#include "cloud_point_rpc/rpc_client.hpp"
|
||||
#include "cloud_point_rpc/tcp_connector.hpp"
|
||||
#include <fstream>
|
||||
#include <jsonrpccxx/common.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
namespace score {
|
||||
|
||||
CloudPointClient::CloudPointClient(std::string ip, int port,
|
||||
StereoAlgorithmType algo,
|
||||
PointCloudBuilder::Options opts,
|
||||
int num_disparities)
|
||||
: ip_(std::move(ip)), port_(port), algo_(algo), opts_(opts),
|
||||
num_disparities_(num_disparities) {}
|
||||
|
||||
CloudPointClient::~CloudPointClient() = default;
|
||||
|
||||
void CloudPointClient::connect() {
|
||||
connector_ = std::make_unique<TCPConnector>(ip_, port_);
|
||||
client_ = std::make_unique<RpcClient>(*connector_);
|
||||
|
||||
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_);
|
||||
builder_ = std::make_unique<PointCloudBuilder>(rectifier_->q(), opts_);
|
||||
}
|
||||
|
||||
bool CloudPointClient::connected() const noexcept {
|
||||
return connector_ != nullptr && client_ != nullptr &&
|
||||
rectifier_ != nullptr && matcher_ != nullptr && builder_ != nullptr;
|
||||
}
|
||||
|
||||
std::expected<PointCloud, CloudPointClient::Error>
|
||||
CloudPointClient::compute_cloud() {
|
||||
if (!connected()) {
|
||||
return std::unexpected(Error{"not connected — call connect() first"});
|
||||
}
|
||||
|
||||
try {
|
||||
// Fetch image pair.
|
||||
const auto pair = client_->get_image_pair();
|
||||
|
||||
// Decode raw bytes into cv::Mat via ImageFactory.
|
||||
auto left_img = ImageFactory::create(pair.left);
|
||||
auto right_img = ImageFactory::create(pair.right);
|
||||
|
||||
// Convert to grayscale according to image type.
|
||||
auto to_gray = [](ImageRPC::Type type,
|
||||
cv::Mat mat) -> std::expected<cv::Mat, Error> {
|
||||
cv::Mat gray;
|
||||
switch (type) {
|
||||
case ImageRPC::Type::BGR:
|
||||
cv::cvtColor(mat, gray, cv::COLOR_BGR2GRAY);
|
||||
return gray;
|
||||
case ImageRPC::Type::RGBA:
|
||||
cv::cvtColor(mat, gray, cv::COLOR_RGBA2GRAY);
|
||||
return gray;
|
||||
case ImageRPC::Type::DEPTH:
|
||||
return std::unexpected(
|
||||
Error{"depth images not supported by stereo pipeline"});
|
||||
default:
|
||||
return std::unexpected(Error{"unknown image type"});
|
||||
}
|
||||
};
|
||||
|
||||
auto left_gray_r = to_gray(pair.left.type, left_img.get());
|
||||
if (!left_gray_r)
|
||||
return std::unexpected(left_gray_r.error());
|
||||
|
||||
auto right_gray_r = to_gray(pair.right.type, right_img.get());
|
||||
if (!right_gray_r)
|
||||
return std::unexpected(right_gray_r.error());
|
||||
|
||||
const cv::Mat &left_gray = *left_gray_r;
|
||||
const cv::Mat &right_gray = *right_gray_r;
|
||||
|
||||
// Rectify, compute disparity, reproject.
|
||||
auto [rect_left, rect_right] =
|
||||
rectifier_->rectify(left_gray, right_gray);
|
||||
const auto disparity = matcher_->compute(rect_left, rect_right);
|
||||
return builder_->build(disparity);
|
||||
|
||||
} catch (const jsonrpccxx::JsonRpcException &e) {
|
||||
return std::unexpected(Error{e.what()});
|
||||
} catch (const std::exception &e) {
|
||||
return std::unexpected(Error{e.what()});
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PLY helper
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void write_ply(const PointCloud &cloud, const std::string &path) {
|
||||
const auto valid = cloud.valid_points();
|
||||
|
||||
std::ofstream out(path);
|
||||
out << "ply\n"
|
||||
<< "format ascii 1.0\n"
|
||||
<< "element vertex " << valid.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";
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace score
|
||||
@ -13,10 +13,12 @@ opencv_cuda_available = cxx.has_header('opencv2/cudastereo.hpp', dependencies: o
|
||||
|
||||
cloud_point_sources = files(
|
||||
'image.cpp',
|
||||
'rectify.cpp',
|
||||
'cpu_stereo_matcher.cpp',
|
||||
'gpu_stereo_matcher.cpp',
|
||||
'stereo_matcher_factory.cpp',
|
||||
'stereo_rectifier.cpp',
|
||||
'point_cloud_builder.cpp',
|
||||
'cloud_point_client.cpp',
|
||||
)
|
||||
|
||||
cpc_deps = [ cloud_point_rpc_dep, opencv_dep ]
|
||||
|
||||
107
src/cloud_point/point_cloud_builder.cpp
Normal file
107
src/cloud_point/point_cloud_builder.cpp
Normal file
@ -0,0 +1,107 @@
|
||||
#include "cloud_point/point_cloud_builder.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
namespace score {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PointCloud::valid_points
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
std::vector<std::array<float, 3>> PointCloud::valid_points() const {
|
||||
std::vector<std::array<float, 3>> pts;
|
||||
pts.reserve(static_cast<size_t>(width * height));
|
||||
for (int i = 0; i < height * width; ++i) {
|
||||
const float x = data[static_cast<size_t>(i) * 3];
|
||||
const float y = data[static_cast<size_t>(i) * 3 + 1];
|
||||
const float z = data[static_cast<size_t>(i) * 3 + 2];
|
||||
if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z)) {
|
||||
pts.push_back({x, y, z});
|
||||
}
|
||||
}
|
||||
return pts;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PointCloudBuilder constructor
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
PointCloudBuilder::PointCloudBuilder(cv::Mat q, Options opts)
|
||||
: q_(std::move(q)), opts_(opts) {
|
||||
if (q_.rows != 4 || q_.cols != 4 || q_.type() != CV_64F) {
|
||||
throw std::invalid_argument("Q matrix must be 4x4 CV_64F, got " +
|
||||
std::to_string(q_.rows) + "x" +
|
||||
std::to_string(q_.cols) +
|
||||
" type=" + std::to_string(q_.type()));
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// PointCloudBuilder::build
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
PointCloud PointCloudBuilder::build(const cv::Mat &disparity) const {
|
||||
cv::Mat disp32;
|
||||
if (disparity.type() == CV_16S) {
|
||||
// SGBM stores disparity in fixed-point with a factor of 16
|
||||
disparity.convertTo(disp32, CV_32F, 1.0 / 16.0);
|
||||
} else if (disparity.type() == CV_32F) {
|
||||
disp32 = disparity;
|
||||
} else {
|
||||
throw std::invalid_argument(
|
||||
"Disparity must be CV_16S or CV_32F, got type=" +
|
||||
std::to_string(disparity.type()));
|
||||
}
|
||||
|
||||
// Reproject to 3-D. handleMissingValues=false is used intentionally:
|
||||
// OpenCV 4.6 incorrectly marks all CV_32F pixels as "missing" when
|
||||
// handleMissingValues=true (empirically verified). Our own filter loop
|
||||
// below already covers all sentinel cases (d≤0, |z|≥10000, depth range).
|
||||
cv::Mat xyz;
|
||||
cv::reprojectImageTo3D(disp32, xyz, q_, /*handleMissingValues=*/false);
|
||||
|
||||
PointCloud cloud;
|
||||
cloud.width = disparity.cols;
|
||||
cloud.height = disparity.rows;
|
||||
cloud.data.resize(static_cast<size_t>(cloud.width) *
|
||||
static_cast<size_t>(cloud.height) * 3u);
|
||||
|
||||
const float kNaN = std::numeric_limits<float>::quiet_NaN();
|
||||
|
||||
for (int r = 0; r < cloud.height; ++r) {
|
||||
for (int c = 0; c < cloud.width; ++c) {
|
||||
const auto &pt = xyz.at<cv::Vec3f>(r, c);
|
||||
const float z = pt[2];
|
||||
const float d = disp32.at<float>(r, c);
|
||||
|
||||
const size_t idx =
|
||||
(static_cast<size_t>(r) * static_cast<size_t>(cloud.width) +
|
||||
static_cast<size_t>(c)) *
|
||||
3u;
|
||||
|
||||
// Mark invalid when: disparity ≤ 0, OpenCV sentinel |z|≥10000,
|
||||
// or depth outside the user-specified range.
|
||||
const bool invalid = (d <= 0.0f) || (std::abs(z) >= 10000.0f) ||
|
||||
(z < opts_.min_depth_m) ||
|
||||
(z > opts_.max_depth_m);
|
||||
|
||||
if (invalid) {
|
||||
cloud.data[idx] = kNaN;
|
||||
cloud.data[idx + 1] = kNaN;
|
||||
cloud.data[idx + 2] = kNaN;
|
||||
} else {
|
||||
cloud.data[idx] = pt[0];
|
||||
cloud.data[idx + 1] = pt[1];
|
||||
cloud.data[idx + 2] = z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return cloud;
|
||||
}
|
||||
|
||||
} // namespace score
|
||||
@ -1,22 +0,0 @@
|
||||
#include "opencv2/calib3d.hpp"
|
||||
#include <cloud_point/rectify.h>
|
||||
|
||||
namespace score {
|
||||
|
||||
Rectify::Rectify() = default;
|
||||
Rectify::~Rectify() = default;
|
||||
/**
|
||||
* @brief perform rectification operation on providen image
|
||||
* @param cameraMatrix matrix of intrinsic params of size 3x3
|
||||
* @param distCoeffs distortion matrix
|
||||
* @param image reference to image object (changed in-place)
|
||||
*/
|
||||
void Rectify::perform(Image &image, cv::Mat cameraMatrix, cv::Mat distCoeffs) {
|
||||
cv::Mat newmatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs,
|
||||
image.get().size(), 1);
|
||||
cv::Mat output;
|
||||
cv::undistort(image.get(), output, cameraMatrix, distCoeffs, newmatrix);
|
||||
output.copyTo(image.get());
|
||||
}
|
||||
|
||||
} // namespace score
|
||||
@ -6,17 +6,17 @@
|
||||
namespace score {
|
||||
|
||||
std::unique_ptr<IStereoMatcher>
|
||||
StereoMatcherFactory::create(StereoAlgorithmType type) {
|
||||
StereoMatcherFactory::create(StereoAlgorithmType type, int num_disparities) {
|
||||
switch (type) {
|
||||
case StereoAlgorithmType::CPU:
|
||||
return std::make_unique<CpuStereoMatcher>();
|
||||
return std::make_unique<CpuStereoMatcher>(0, num_disparities);
|
||||
case StereoAlgorithmType::GPU:
|
||||
try {
|
||||
return std::make_unique<GpuStereoMatcher>();
|
||||
} catch (const std::exception &e) {
|
||||
LOG(WARNING) << "GPU stereo matcher unavailable: " << e.what()
|
||||
<< ". Falling back to CPU.";
|
||||
return std::make_unique<CpuStereoMatcher>();
|
||||
return std::make_unique<CpuStereoMatcher>(0, num_disparities);
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
|
||||
90
src/cloud_point/stereo_rectifier.cpp
Normal file
90
src/cloud_point/stereo_rectifier.cpp
Normal file
@ -0,0 +1,90 @@
|
||||
#include "cloud_point/stereo_rectifier.hpp"
|
||||
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "cloud_point/matrixFactory.h"
|
||||
|
||||
namespace score {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Internal helpers
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
void require_mat(const cv::Mat &mat, int rows, int cols, int type,
|
||||
const char *name) {
|
||||
if (mat.rows != rows || mat.cols != cols || mat.type() != type) {
|
||||
throw std::invalid_argument(
|
||||
std::string(name) + " must be " + std::to_string(rows) + "x" +
|
||||
std::to_string(cols) + " CV_64F (type " + std::to_string(CV_64F) +
|
||||
"), got " + std::to_string(mat.rows) + "x" +
|
||||
std::to_string(mat.cols) + " type=" + std::to_string(mat.type()));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// StereoRectifier::Calibration::from_rpc
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
StereoRectifier::Calibration
|
||||
StereoRectifier::Calibration::from_rpc(const StereoCalibrationRPC &rpc) {
|
||||
Calibration calib;
|
||||
calib.k_left = CameraMatrixFactory::create<3, 3>(rpc.left.camera_matrix);
|
||||
calib.d_left = CameraMatrixFactory::create<1, 5>(rpc.left.dist_coeffs);
|
||||
calib.k_right = CameraMatrixFactory::create<3, 3>(rpc.right.camera_matrix);
|
||||
calib.d_right = CameraMatrixFactory::create<1, 5>(rpc.right.dist_coeffs);
|
||||
calib.r = CameraMatrixFactory::create<3, 3>(rpc.rotation);
|
||||
calib.t = CameraMatrixFactory::create<3, 1>(rpc.translation);
|
||||
calib.image_size = cv::Size(rpc.width, rpc.height);
|
||||
return calib;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// StereoRectifier constructor
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
StereoRectifier::StereoRectifier(const Calibration &calib) {
|
||||
require_mat(calib.k_left, 3, 3, CV_64F, "k_left");
|
||||
require_mat(calib.k_right, 3, 3, CV_64F, "k_right");
|
||||
require_mat(calib.d_left, 1, 5, CV_64F, "d_left");
|
||||
require_mat(calib.d_right, 1, 5, CV_64F, "d_right");
|
||||
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::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_,
|
||||
cv::CALIB_ZERO_DISPARITY, /*alpha=*/0);
|
||||
|
||||
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_);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// StereoRectifier::rectify
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
std::pair<cv::Mat, cv::Mat>
|
||||
StereoRectifier::rectify(const cv::Mat &left, const cv::Mat &right) const {
|
||||
cv::Mat rect_left, rect_right;
|
||||
cv::remap(left, rect_left, map_lx_, map_ly_, cv::INTER_LINEAR);
|
||||
cv::remap(right, rect_right, map_rx_, map_ry_, cv::INTER_LINEAR);
|
||||
return {rect_left, rect_right};
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// StereoRectifier::q
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
const cv::Mat &StereoRectifier::q() const noexcept { return q_; }
|
||||
|
||||
} // namespace score
|
||||
@ -40,18 +40,30 @@ cloud_point_rpc_test_dep = declare_dependency(
|
||||
dependencies: [cloud_point_rpc_dep],
|
||||
)
|
||||
|
||||
subdir('cloud_point')
|
||||
|
||||
# CLI lib — links cloud_point_compute when OpenCV is available so that
|
||||
# options 4 and 5 (compute-cloud) are compiled in.
|
||||
cli_deps = [cloud_point_rpc_dep]
|
||||
cli_cpp_args = []
|
||||
if opencv_dep.found()
|
||||
cli_deps += [cloud_point_compute_dep]
|
||||
cli_cpp_args += ['-DHAVE_CLOUD_POINT_COMPUTE']
|
||||
endif
|
||||
|
||||
libcloud_point_rpc_cli = shared_library(
|
||||
'libcloud_point_rpc_cli',
|
||||
'cli.cpp',
|
||||
include_directories: inc,
|
||||
dependencies: [cloud_point_rpc_dep],
|
||||
dependencies: cli_deps,
|
||||
cpp_args: cli_cpp_args,
|
||||
install: true,
|
||||
)
|
||||
|
||||
cloud_point_rpc_cli_dep = declare_dependency(
|
||||
include_directories: inc,
|
||||
link_with: libcloud_point_rpc_cli,
|
||||
dependencies: [cloud_point_rpc_dep],
|
||||
dependencies: cli_deps,
|
||||
)
|
||||
|
||||
# Client/CLI tool (legacy stdin/stdout)
|
||||
@ -80,5 +92,3 @@ executable(
|
||||
dependencies: cloud_point_rpc_dep,
|
||||
install: true,
|
||||
)
|
||||
|
||||
subdir('cloud_point')
|
||||
|
||||
@ -15,7 +15,10 @@ if opencv_dep.found()
|
||||
message('found cloud_point_compute dependency')
|
||||
test_sources += files(
|
||||
'test_image.cpp',
|
||||
'test_stereo_matcher.cpp'
|
||||
'test_stereo_matcher.cpp',
|
||||
'test_stereo_rectifier.cpp',
|
||||
'test_point_cloud_builder.cpp',
|
||||
'test_cloud_point_client.cpp'
|
||||
)
|
||||
test_deps += [cloud_point_compute_dep]
|
||||
else
|
||||
|
||||
294
tests/test_cloud_point_client.cpp
Normal file
294
tests/test_cloud_point_client.cpp
Normal file
@ -0,0 +1,294 @@
|
||||
// E2E tests for CloudPointClient: in-process TcpServer + RpcServer with a
|
||||
// synthetic scene providing known ground-truth depth.
|
||||
//
|
||||
// Calibration: fx=fy=800, cx=320, cy=240, 640×480, baseline=0.06 m,
|
||||
// R=I, T=[-0.06,0,0]. Disparity = 32 px → z = 1.5 m.
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <sstream>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "cloud_point/cloud_point_client.hpp"
|
||||
#include "cloud_point_rpc/cli.hpp"
|
||||
#include "cloud_point_rpc/rpc_dto.hpp"
|
||||
#include "cloud_point_rpc/rpc_server.hpp"
|
||||
#include "cloud_point_rpc/tcp_server.hpp"
|
||||
|
||||
using namespace score;
|
||||
using json = nlohmann::json;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Shared synthetic calibration helpers
|
||||
// ---------------------------------------------------------------------------
|
||||
namespace {
|
||||
|
||||
constexpr double kFx = 800.0;
|
||||
constexpr double kFy = 800.0;
|
||||
constexpr double kCx = 320.0;
|
||||
constexpr double kCy = 240.0;
|
||||
constexpr int kWidth = 640;
|
||||
constexpr int kHeight = 480;
|
||||
constexpr double kBaseline = 0.06;
|
||||
constexpr double kTx = -kBaseline; // OpenCV T convention
|
||||
|
||||
/// Standard stereo calibration DTO (parallel rig, no distortion).
|
||||
StereoCalibrationRPC make_stereo_calib() {
|
||||
StereoCalibrationRPC c;
|
||||
c.width = kWidth;
|
||||
c.height = kHeight;
|
||||
c.left.camera_matrix = {kFx, 0, kCx, 0, kFy, kCy, 0, 0, 1};
|
||||
c.left.dist_coeffs = {0, 0, 0, 0, 0};
|
||||
c.right = c.left;
|
||||
c.rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
||||
c.translation = {kTx, 0.0, 0.0};
|
||||
return c;
|
||||
}
|
||||
|
||||
/// Synthetic image pair: random left (seed 42), right = left shifted +32 px.
|
||||
/// Convention: right(x, y) = left(x+d, y) → disparity d = 32 for positive
|
||||
/// depth (matches service.cpp and OpenCV SGBM sign convention with Tx < 0).
|
||||
ImagePairRPC make_image_pair(int disparity = 32) {
|
||||
// Generate textured left grayscale image with fixed seed.
|
||||
cv::theRNG().state = 42;
|
||||
cv::Mat left_gray(kHeight, kWidth, CV_8UC1);
|
||||
cv::randu(left_gray, 0, 256);
|
||||
|
||||
// Build right by shifting: right(x) = left(x+d), 0 at right border.
|
||||
cv::Mat right_gray = cv::Mat::zeros(kHeight, kWidth, CV_8UC1);
|
||||
for (int y = 0; y < kHeight; ++y) {
|
||||
for (int x = 0; x < kWidth - disparity; ++x) {
|
||||
right_gray.at<uchar>(y, x) = left_gray.at<uchar>(y, x + disparity);
|
||||
}
|
||||
}
|
||||
|
||||
// Convert to BGR for the wire protocol.
|
||||
cv::Mat left_bgr, right_bgr;
|
||||
cv::cvtColor(left_gray, left_bgr, cv::COLOR_GRAY2BGR);
|
||||
cv::cvtColor(right_gray, right_bgr, cv::COLOR_GRAY2BGR);
|
||||
|
||||
auto mat_to_rpc = [](const cv::Mat &img, int w, int h) -> ImageRPC {
|
||||
ImageRPC rpc;
|
||||
rpc.width = w;
|
||||
rpc.height = h;
|
||||
rpc.type = ImageRPC::Type::BGR;
|
||||
const size_t sz = static_cast<size_t>(w) * h * 3;
|
||||
rpc.data.resize(sz);
|
||||
std::memcpy(rpc.data.data(), img.data, sz);
|
||||
return rpc;
|
||||
};
|
||||
|
||||
ImagePairRPC pair;
|
||||
pair.frame = 0;
|
||||
pair.left = mat_to_rpc(left_bgr, kWidth, kHeight);
|
||||
pair.right = mat_to_rpc(right_bgr, kWidth, kHeight);
|
||||
return pair;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Test fixture: in-process TcpServer + RpcServer on port 9201
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
class CloudPointClientTest : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() override {
|
||||
FLAGS_logtostderr = true;
|
||||
if (!google::IsGoogleLoggingInitialized())
|
||||
google::InitGoogleLogging("TestCloudPointClient");
|
||||
}
|
||||
void TearDown() override {
|
||||
if (server_) {
|
||||
server_->stop();
|
||||
}
|
||||
}
|
||||
|
||||
/// Start an in-process server with the given RpcServer and wait for it.
|
||||
void start_server(int port, std::unique_ptr<RpcServer> rpc) {
|
||||
rpc_server_ = std::move(rpc);
|
||||
server_ = std::make_unique<TcpServer>(
|
||||
"127.0.0.1", port, [this](const std::string &req) {
|
||||
return rpc_server_->process(req);
|
||||
});
|
||||
server_->start();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
std::unique_ptr<RpcServer> rpc_server_;
|
||||
std::unique_ptr<TcpServer> server_;
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Test 1: connect succeeds; compute_cloud() returns a cloud with median z ≈ 1.5
|
||||
// m
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(CloudPointClientTest, ComputeCloudReturnsCorrectDepth) {
|
||||
constexpr int kPort = 9201;
|
||||
|
||||
// Build RPC server with known stereo calibration and image pair.
|
||||
auto rpc = std::make_unique<RpcServer>();
|
||||
rpc->register_method("get-stereo-calibration", [](const json &) -> json {
|
||||
json j;
|
||||
to_json(j, make_stereo_calib());
|
||||
return j;
|
||||
});
|
||||
rpc->register_method("get-image-pair", [](const json &) -> json {
|
||||
json j;
|
||||
to_json(j, make_image_pair(32));
|
||||
return j;
|
||||
});
|
||||
start_server(kPort, std::move(rpc));
|
||||
|
||||
CloudPointClient client("127.0.0.1", kPort, StereoAlgorithmType::CPU);
|
||||
ASSERT_NO_THROW(client.connect());
|
||||
ASSERT_TRUE(client.connected());
|
||||
|
||||
auto result = client.compute_cloud();
|
||||
ASSERT_TRUE(result.has_value())
|
||||
<< "compute_cloud returned Error: " << result.error().message;
|
||||
|
||||
const auto &cloud = *result;
|
||||
EXPECT_EQ(cloud.width, kWidth);
|
||||
EXPECT_EQ(cloud.height, kHeight);
|
||||
EXPECT_FALSE(cloud.valid_points().empty())
|
||||
<< "Expected at least some valid points";
|
||||
|
||||
// Collect z values in the central region (avoid SGBM borders).
|
||||
// Horizontal: [150, 490) to skip left border (SGBM invalid) and
|
||||
// right border where right image has no data (last 32 cols).
|
||||
// Vertical: [50, 430).
|
||||
constexpr int kXMin = 150;
|
||||
constexpr int kXMax = 490;
|
||||
constexpr int kYMin = 50;
|
||||
constexpr int kYMax = 430;
|
||||
|
||||
std::vector<float> z_vals;
|
||||
z_vals.reserve(static_cast<size_t>((kXMax - kXMin) * (kYMax - kYMin)));
|
||||
|
||||
for (int y = kYMin; y < kYMax; ++y) {
|
||||
for (int x = kXMin; x < kXMax; ++x) {
|
||||
const float z =
|
||||
cloud.data[static_cast<size_t>(y * kWidth + x) * 3 + 2];
|
||||
if (!std::isnan(z))
|
||||
z_vals.push_back(z);
|
||||
}
|
||||
}
|
||||
|
||||
ASSERT_FALSE(z_vals.empty()) << "No valid points in central region";
|
||||
|
||||
// Compute median z.
|
||||
const auto mid = z_vals.begin() + static_cast<ptrdiff_t>(z_vals.size() / 2);
|
||||
std::nth_element(z_vals.begin(), mid, z_vals.end());
|
||||
const float median_z = *mid;
|
||||
|
||||
constexpr float kExpectedZ = static_cast<float>(kFx * kBaseline / 32.0);
|
||||
constexpr float kToleranceZ = kExpectedZ * 0.05f; // 5%
|
||||
|
||||
EXPECT_NEAR(median_z, kExpectedZ, kToleranceZ)
|
||||
<< "Median z in central region should be ~" << kExpectedZ
|
||||
<< " m (expected=" << kExpectedZ << ", got=" << median_z << ")";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Test 2: server returns garbage image data → compute_cloud() returns Error
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(CloudPointClientTest, GarbageImageDataReturnsError) {
|
||||
constexpr int kPort = 9202;
|
||||
|
||||
auto rpc = std::make_unique<RpcServer>();
|
||||
rpc->register_method("get-stereo-calibration", [](const json &) -> json {
|
||||
json j;
|
||||
to_json(j, make_stereo_calib());
|
||||
return j;
|
||||
});
|
||||
// Return an image pair whose data size doesn't match width*height*channels.
|
||||
rpc->register_method("get-image-pair", [](const json &) -> json {
|
||||
ImagePairRPC bad_pair;
|
||||
bad_pair.frame = 0;
|
||||
bad_pair.left.width = kWidth;
|
||||
bad_pair.left.height = kHeight;
|
||||
bad_pair.left.type = ImageRPC::Type::BGR;
|
||||
bad_pair.left.data = {0x01, 0x02}; // wrong size: 2 bytes, not 640*480*3
|
||||
bad_pair.right = bad_pair.left;
|
||||
json j;
|
||||
to_json(j, bad_pair);
|
||||
return j;
|
||||
});
|
||||
start_server(kPort, std::move(rpc));
|
||||
|
||||
CloudPointClient client("127.0.0.1", kPort, StereoAlgorithmType::CPU);
|
||||
ASSERT_NO_THROW(client.connect());
|
||||
|
||||
auto result = client.compute_cloud();
|
||||
EXPECT_FALSE(result.has_value())
|
||||
<< "Expected Error for garbage image data, got a cloud instead";
|
||||
if (!result.has_value()) {
|
||||
EXPECT_FALSE(result.error().message.empty());
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Test 3: connect() to a closed port throws
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(CloudPointClientTest, ConnectToClosedPortThrows) {
|
||||
// Port 9203 has no server running.
|
||||
CloudPointClient client("127.0.0.1", 9203, StereoAlgorithmType::CPU);
|
||||
EXPECT_THROW(client.connect(), std::runtime_error);
|
||||
EXPECT_FALSE(client.connected());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Test 4: CLI smoke — option 4 against the mock server
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST_F(CloudPointClientTest, CliOption4ComputeCloud) {
|
||||
constexpr int kPort = 9204;
|
||||
|
||||
auto rpc = std::make_unique<RpcServer>();
|
||||
rpc->register_method("get-stereo-calibration", [](const json &) -> json {
|
||||
json j;
|
||||
to_json(j, make_stereo_calib());
|
||||
return j;
|
||||
});
|
||||
rpc->register_method("get-image-pair", [](const json &) -> json {
|
||||
json j;
|
||||
to_json(j, make_image_pair(32));
|
||||
return j;
|
||||
});
|
||||
// The CLI also uses the legacy RPC methods; register stubs so it can
|
||||
// connect.
|
||||
rpc->register_method("get-intrinsic-params",
|
||||
[](const json &) { return std::vector<double>{1.0}; });
|
||||
rpc->register_method("get-extrinsic-params",
|
||||
[](const json &) { return std::vector<double>{1.0}; });
|
||||
rpc->register_method("get-cloud-point", [](const json &) {
|
||||
return std::vector<std::vector<double>>{};
|
||||
});
|
||||
start_server(kPort, std::move(rpc));
|
||||
|
||||
std::istringstream input("4\n0\n");
|
||||
std::ostringstream output;
|
||||
|
||||
const int rc = run_cli(input, output, "127.0.0.1", kPort);
|
||||
EXPECT_EQ(rc, 0);
|
||||
|
||||
const std::string out = output.str();
|
||||
// Expect either a point count line ("valid_pts=") or an error message.
|
||||
EXPECT_THAT(out, ::testing::AnyOf(::testing::HasSubstr("valid_pts="),
|
||||
::testing::HasSubstr("Error")))
|
||||
<< "CLI output was:\n"
|
||||
<< out;
|
||||
}
|
||||
@ -1,7 +1,6 @@
|
||||
//
|
||||
// Created by vptyp on 12.03.2026.
|
||||
//
|
||||
#include "cloud_point/rectify.h"
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
@ -159,18 +158,3 @@ TEST_F(ImageTest, CameraMatrixCreateThrowsOnWrongSize) {
|
||||
EXPECT_THROW((score::CameraMatrixFactory::create<3, 3>(vals)),
|
||||
std::runtime_error);
|
||||
}
|
||||
|
||||
TEST_F(ImageTest, RectificationNoThrow) {
|
||||
score::Rectify rectify;
|
||||
score::ImageRPC rpc;
|
||||
rpc.width = 10;
|
||||
rpc.height = 20;
|
||||
rpc.type = score::ImageRPC::Type::BGR;
|
||||
rpc.data.resize(rpc.width * rpc.height * 3, 128);
|
||||
score::Image image = score::ImageFactory::create(rpc);
|
||||
double fx = 10.1, fy = 20.2, cx = 30.3, cy = 40.4;
|
||||
cv::Mat cameraMatrix =
|
||||
(cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
|
||||
|
||||
EXPECT_NO_THROW(rectify.perform(image, cameraMatrix));
|
||||
}
|
||||
|
||||
254
tests/test_point_cloud_builder.cpp
Normal file
254
tests/test_point_cloud_builder.cpp
Normal file
@ -0,0 +1,254 @@
|
||||
// Tests for PointCloudBuilder: CV_16S / CV_32F handling, depth filtering,
|
||||
// NaN propagation, and valid_points() correctness.
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
#include "cloud_point/point_cloud_builder.hpp"
|
||||
#include "cloud_point/stereo_rectifier.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Shared synthetic calibration (same as test_stereo_rectifier.cpp)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
constexpr double kFx = 800.0;
|
||||
constexpr double kFy = 800.0;
|
||||
constexpr double kCx = 320.0;
|
||||
constexpr double kCy = 240.0;
|
||||
constexpr int kWidth = 640;
|
||||
constexpr int kHeight = 480;
|
||||
constexpr double kBaseline = 0.06; // metres
|
||||
constexpr double kTx = -kBaseline;
|
||||
|
||||
score::StereoRectifier::Calibration make_calib() {
|
||||
score::StereoRectifier::Calibration calib;
|
||||
// clang-format off
|
||||
calib.k_left = (cv::Mat_<double>(3, 3) <<
|
||||
kFx, 0, kCx,
|
||||
0, kFy, kCy,
|
||||
0, 0, 1);
|
||||
calib.k_right = calib.k_left.clone();
|
||||
calib.d_left = cv::Mat::zeros(1, 5, CV_64F);
|
||||
calib.d_right = cv::Mat::zeros(1, 5, CV_64F);
|
||||
calib.r = cv::Mat::eye(3, 3, CV_64F);
|
||||
calib.t = (cv::Mat_<double>(3, 1) << kTx, 0.0, 0.0);
|
||||
// clang-format on
|
||||
calib.image_size = cv::Size(kWidth, kHeight);
|
||||
return calib;
|
||||
}
|
||||
|
||||
cv::Mat make_q() {
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
return rectifier.q().clone();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Helper: count non-NaN (valid) pixels in the cloud
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
int count_valid(const score::PointCloud &cloud) {
|
||||
int n = 0;
|
||||
for (int i = 0; i < cloud.height * cloud.width; ++i) {
|
||||
if (!std::isnan(cloud.data[static_cast<size_t>(i) * 3]))
|
||||
++n;
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Constant-disparity CV_16S → expected depth = fx * baseline / disparity
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, ConstantDisparityCV16S) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
// d = 32 px, stored as SGBM fixed-point (d * 16)
|
||||
constexpr float kDisp = 32.0f;
|
||||
constexpr float kExpectedZ =
|
||||
static_cast<float>(kFx * kBaseline / kDisp); // 1.5 m
|
||||
|
||||
cv::Mat disparity(kHeight, kWidth, CV_16S,
|
||||
cv::Scalar(static_cast<short>(kDisp * 16)));
|
||||
|
||||
const auto cloud = builder.build(disparity);
|
||||
|
||||
EXPECT_EQ(cloud.width, kWidth);
|
||||
EXPECT_EQ(cloud.height, kHeight);
|
||||
EXPECT_EQ(static_cast<int>(cloud.data.size()), kWidth * kHeight * 3);
|
||||
|
||||
// Every point should be valid and at z ≈ 1.5 m
|
||||
const auto valid = cloud.valid_points();
|
||||
EXPECT_EQ(static_cast<int>(valid.size()), kWidth * kHeight);
|
||||
|
||||
for (const auto &pt : valid) {
|
||||
EXPECT_NEAR(pt[2], kExpectedZ, 1e-3f)
|
||||
<< "z should be fx*baseline/disparity = " << kExpectedZ << " m";
|
||||
}
|
||||
}
|
||||
|
||||
TEST(PointCloudBuilderTest, CenterPixelXYNearZero) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
constexpr float kDisp = 32.0f;
|
||||
cv::Mat disparity(kHeight, kWidth, CV_16S,
|
||||
cv::Scalar(static_cast<short>(kDisp * 16)));
|
||||
|
||||
const auto cloud = builder.build(disparity);
|
||||
|
||||
// At the image center (row=cy=240, col=cx=320), x≈0 and y≈0.
|
||||
const int center_idx =
|
||||
(static_cast<int>(kCy) * kWidth + static_cast<int>(kCx)) * 3;
|
||||
EXPECT_NEAR(cloud.data[static_cast<size_t>(center_idx)], 0.0f, 0.05f)
|
||||
<< "x at image center should be near 0";
|
||||
EXPECT_NEAR(cloud.data[static_cast<size_t>(center_idx) + 1], 0.0f, 0.05f)
|
||||
<< "y at image center should be near 0";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Zero / negative disparity → NaN
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, ZeroDisparityProducesNaN) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
cv::Mat disparity = cv::Mat::zeros(kHeight, kWidth, CV_16S);
|
||||
const auto cloud = builder.build(disparity);
|
||||
|
||||
EXPECT_EQ(count_valid(cloud), 0)
|
||||
<< "All points should be NaN when disparity is zero";
|
||||
EXPECT_EQ(static_cast<int>(cloud.valid_points().size()), 0);
|
||||
}
|
||||
|
||||
TEST(PointCloudBuilderTest, NegativeDisparityProducesNaN) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
cv::Mat disparity(kHeight, kWidth, CV_16S, cv::Scalar(-16)); // -1 px
|
||||
const auto cloud = builder.build(disparity);
|
||||
|
||||
EXPECT_EQ(count_valid(cloud), 0)
|
||||
<< "All points should be NaN when disparity is negative";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Out-of-range depth filter → all NaN
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, MaxDepthFilterRejectsAll) {
|
||||
const cv::Mat q = make_q();
|
||||
// kExpectedZ = 1.5 m; set max_depth_m = 1.0 to reject all points.
|
||||
score::PointCloudBuilder::Options opts;
|
||||
opts.max_depth_m = 1.0f;
|
||||
score::PointCloudBuilder builder(q, opts);
|
||||
|
||||
constexpr float kDisp = 32.0f; // → z = 1.5 m, which exceeds max
|
||||
cv::Mat disparity(kHeight, kWidth, CV_16S,
|
||||
cv::Scalar(static_cast<short>(kDisp * 16)));
|
||||
|
||||
const auto cloud = builder.build(disparity);
|
||||
EXPECT_EQ(count_valid(cloud), 0)
|
||||
<< "All points should be NaN when z=1.5 m exceeds max_depth_m=1.0 m";
|
||||
}
|
||||
|
||||
TEST(PointCloudBuilderTest, MinDepthFilterRejectsAll) {
|
||||
const cv::Mat q = make_q();
|
||||
// kExpectedZ = 1.5 m; set min_depth_m = 2.0 to reject all points.
|
||||
score::PointCloudBuilder::Options opts;
|
||||
opts.min_depth_m = 2.0f;
|
||||
score::PointCloudBuilder builder(q, opts);
|
||||
|
||||
constexpr float kDisp = 32.0f; // → z = 1.5 m, below min
|
||||
cv::Mat disparity(kHeight, kWidth, CV_16S,
|
||||
cv::Scalar(static_cast<short>(kDisp * 16)));
|
||||
|
||||
const auto cloud = builder.build(disparity);
|
||||
EXPECT_EQ(count_valid(cloud), 0)
|
||||
<< "All points should be NaN when z=1.5 m is below min_depth_m=2.0 m";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// valid_points() count matches non-NaN count
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, ValidPointsCountMatchesNonNaN) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
// Half the image has positive disparity, half has zero
|
||||
cv::Mat disparity(kHeight, kWidth, CV_16S, cv::Scalar(0));
|
||||
const cv::Rect left_half(0, 0, kWidth / 2, kHeight);
|
||||
disparity(left_half).setTo(cv::Scalar(32 * 16));
|
||||
|
||||
const auto cloud = builder.build(disparity);
|
||||
|
||||
const int manual_count = count_valid(cloud);
|
||||
const int method_count = static_cast<int>(cloud.valid_points().size());
|
||||
EXPECT_EQ(manual_count, method_count)
|
||||
<< "valid_points() size must equal the number of non-NaN pixels";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// CV_32F input works identically
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, CV32FInputMatchesCV16S) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
constexpr float kDisp = 32.0f;
|
||||
|
||||
// CV_16S reference
|
||||
cv::Mat disp16(kHeight, kWidth, CV_16S,
|
||||
cv::Scalar(static_cast<short>(kDisp * 16)));
|
||||
const auto cloud16 = builder.build(disp16);
|
||||
|
||||
// CV_32F equivalent
|
||||
cv::Mat disp32(kHeight, kWidth, CV_32F, cv::Scalar(kDisp));
|
||||
const auto cloud32 = builder.build(disp32);
|
||||
|
||||
ASSERT_EQ(cloud16.data.size(), cloud32.data.size());
|
||||
|
||||
// Every z-coordinate should match within floating-point tolerance
|
||||
for (int i = 0; i < kHeight * kWidth; ++i) {
|
||||
const float z16 = cloud16.data[static_cast<size_t>(i) * 3 + 2];
|
||||
const float z32 = cloud32.data[static_cast<size_t>(i) * 3 + 2];
|
||||
EXPECT_NEAR(z16, z32, 1e-3f) << "at pixel " << i;
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Unsupported disparity type → throws
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, CV8UInputThrows) {
|
||||
const cv::Mat q = make_q();
|
||||
score::PointCloudBuilder builder(q);
|
||||
|
||||
cv::Mat bad_disparity(kHeight, kWidth, CV_8U, cv::Scalar(32));
|
||||
EXPECT_THROW(builder.build(bad_disparity), std::invalid_argument);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Invalid Q matrix → constructor throws
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(PointCloudBuilderTest, InvalidQSizeThrows) {
|
||||
cv::Mat bad_q = cv::Mat::eye(3, 4, CV_64F); // not 4x4
|
||||
EXPECT_THROW(score::PointCloudBuilder{bad_q}, std::invalid_argument);
|
||||
}
|
||||
|
||||
TEST(PointCloudBuilderTest, InvalidQTypeThrows) {
|
||||
cv::Mat bad_q = cv::Mat::eye(4, 4, CV_32F); // not CV_64F
|
||||
EXPECT_THROW(score::PointCloudBuilder{bad_q}, std::invalid_argument);
|
||||
}
|
||||
242
tests/test_stereo_rectifier.cpp
Normal file
242
tests/test_stereo_rectifier.cpp
Normal file
@ -0,0 +1,242 @@
|
||||
// Tests for StereoRectifier: construction, Q-matrix semantics, from_rpc
|
||||
// round-trip, near-identity rectification, and invalid-calibration rejection.
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "cloud_point/stereo_rectifier.hpp"
|
||||
#include "cloud_point_rpc/rpc_dto.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Helpers — synthetic calibration
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
// fx = fy = 800, cx = 320, cy = 240, 640x480, zero distortion,
|
||||
// R = identity, T = [-0.06, 0, 0] (6 cm horizontal baseline).
|
||||
constexpr double kFx = 800.0;
|
||||
constexpr double kFy = 800.0;
|
||||
constexpr double kCx = 320.0;
|
||||
constexpr double kCy = 240.0;
|
||||
constexpr int kWidth = 640;
|
||||
constexpr int kHeight = 480;
|
||||
constexpr double kBaseline = 0.06; // metres
|
||||
constexpr double kTx = -kBaseline; // OpenCV T: left→right camera
|
||||
|
||||
score::StereoRectifier::Calibration make_calib() {
|
||||
score::StereoRectifier::Calibration calib;
|
||||
|
||||
// clang-format off
|
||||
calib.k_left = (cv::Mat_<double>(3, 3) <<
|
||||
kFx, 0, kCx,
|
||||
0, kFy, kCy,
|
||||
0, 0, 1);
|
||||
calib.k_right = calib.k_left.clone();
|
||||
|
||||
calib.d_left = cv::Mat::zeros(1, 5, CV_64F);
|
||||
calib.d_right = cv::Mat::zeros(1, 5, CV_64F);
|
||||
|
||||
calib.r = cv::Mat::eye(3, 3, CV_64F);
|
||||
|
||||
calib.t = (cv::Mat_<double>(3, 1) << kTx, 0.0, 0.0);
|
||||
// clang-format on
|
||||
|
||||
calib.image_size = cv::Size(kWidth, kHeight);
|
||||
return calib;
|
||||
}
|
||||
|
||||
score::StereoCalibrationRPC make_rpc() {
|
||||
score::StereoCalibrationRPC rpc;
|
||||
// Camera matrix: row-major 3x3
|
||||
rpc.left.camera_matrix = {kFx, 0, kCx, 0, kFy, kCy, 0, 0, 1};
|
||||
rpc.right.camera_matrix = rpc.left.camera_matrix;
|
||||
rpc.left.dist_coeffs = {0, 0, 0, 0, 0};
|
||||
rpc.right.dist_coeffs = {0, 0, 0, 0, 0};
|
||||
rpc.rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
||||
rpc.translation = {kTx, 0.0, 0.0};
|
||||
rpc.width = kWidth;
|
||||
rpc.height = kHeight;
|
||||
return rpc;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Construction & Q-matrix semantics
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(StereoRectifierTest, QMatrixIs4x4) {
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
const cv::Mat &q = rectifier.q();
|
||||
EXPECT_EQ(q.rows, 4);
|
||||
EXPECT_EQ(q.cols, 4);
|
||||
EXPECT_EQ(q.type(), CV_64F);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, QMatrixFocalEntry) {
|
||||
// OpenCV Q layout (horizontal stereo):
|
||||
// Q = [1 0 0 -cx ]
|
||||
// [0 1 0 -cy ]
|
||||
// [0 0 0 f ] ← Q(2,3) = focal length
|
||||
// [0 0 -1/Tx ...]
|
||||
// With fx = fy = 800, Q(2,3) should be ≈ 800.
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
const cv::Mat &q = rectifier.q();
|
||||
EXPECT_NEAR(q.at<double>(2, 3), kFx, 1.0)
|
||||
<< "Q(2,3) should equal the focal length (~" << kFx << ")";
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, QMatrixBaselineEntry) {
|
||||
// Q(3,2) = -1/Tx. With Tx = -0.06, -1/Tx = +16.667 (positive).
|
||||
// Observed sign convention: Q(3,2) > 0 for Tx < 0.
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
const cv::Mat &q = rectifier.q();
|
||||
|
||||
const double expected = -1.0 / kTx; // = +16.667 for Tx=-0.06
|
||||
EXPECT_NEAR(q.at<double>(3, 2), expected, 1.0)
|
||||
<< "Q(3,2) should be -1/Tx = " << expected
|
||||
<< " (observed sign: positive for Tx < 0)";
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, DepthFromQConsistency) {
|
||||
// z = Q(2,3) / (-Q(3,2)) / disparity → = fx * |Tx| / disparity.
|
||||
// For d=32: z = 800 * 0.06 / 32 = 1.5 m.
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
const cv::Mat &q = rectifier.q();
|
||||
|
||||
const double f = q.at<double>(2, 3);
|
||||
const double q32 = q.at<double>(3, 2);
|
||||
// baseline_in_Q = -1/q32 gives Tx, |Tx| = baseline
|
||||
const double baseline_q = std::abs(-1.0 / q32);
|
||||
const double disparity = 32.0;
|
||||
const double z = f * baseline_q / disparity;
|
||||
|
||||
EXPECT_NEAR(z, 1.5, 1e-3)
|
||||
<< "z = fx * baseline / disparity = 800 * 0.06 / 32 should be 1.5 m";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// from_rpc round-trip
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(StereoRectifierTest, FromRpcRoundTrip) {
|
||||
const auto rpc = make_rpc();
|
||||
const auto calib = score::StereoRectifier::Calibration::from_rpc(rpc);
|
||||
|
||||
// Sizes
|
||||
EXPECT_EQ(calib.k_left.rows, 3);
|
||||
EXPECT_EQ(calib.k_left.cols, 3);
|
||||
EXPECT_EQ(calib.d_left.rows, 1);
|
||||
EXPECT_EQ(calib.d_left.cols, 5);
|
||||
EXPECT_EQ(calib.r.rows, 3);
|
||||
EXPECT_EQ(calib.r.cols, 3);
|
||||
EXPECT_EQ(calib.t.rows, 3);
|
||||
EXPECT_EQ(calib.t.cols, 1);
|
||||
EXPECT_EQ(calib.image_size, cv::Size(kWidth, kHeight));
|
||||
|
||||
// Values — camera matrix diagonal
|
||||
EXPECT_DOUBLE_EQ(calib.k_left.at<double>(0, 0), kFx);
|
||||
EXPECT_DOUBLE_EQ(calib.k_left.at<double>(1, 1), kFy);
|
||||
EXPECT_DOUBLE_EQ(calib.k_left.at<double>(0, 2), kCx);
|
||||
EXPECT_DOUBLE_EQ(calib.k_left.at<double>(1, 2), kCy);
|
||||
|
||||
// Translation
|
||||
EXPECT_DOUBLE_EQ(calib.t.at<double>(0, 0), kTx);
|
||||
EXPECT_DOUBLE_EQ(calib.t.at<double>(1, 0), 0.0);
|
||||
EXPECT_DOUBLE_EQ(calib.t.at<double>(2, 0), 0.0);
|
||||
|
||||
// Rotation is identity
|
||||
for (int r = 0; r < 3; ++r)
|
||||
for (int c = 0; c < 3; ++c)
|
||||
EXPECT_DOUBLE_EQ(calib.r.at<double>(r, c), r == c ? 1.0 : 0.0);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, FromRpcProducesValidRectifier) {
|
||||
const auto rpc = make_rpc();
|
||||
const auto calib = score::StereoRectifier::Calibration::from_rpc(rpc);
|
||||
EXPECT_NO_THROW(score::StereoRectifier rectifier(calib));
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Near-identity rectification
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(StereoRectifierTest, RectifyOutputSizeUnchanged) {
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
|
||||
// Deterministic gradient image (use fixed seed via theRNG)
|
||||
cv::theRNG().state = 42;
|
||||
cv::Mat left(kHeight, kWidth, CV_8UC1);
|
||||
cv::randn(left, 128, 40);
|
||||
cv::Mat right = left.clone();
|
||||
|
||||
auto [rl, rr] = rectifier.rectify(left, right);
|
||||
|
||||
EXPECT_EQ(rl.rows, kHeight);
|
||||
EXPECT_EQ(rl.cols, kWidth);
|
||||
EXPECT_EQ(rr.rows, kHeight);
|
||||
EXPECT_EQ(rr.cols, kWidth);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, RectifyNearIdentityWithZeroDistortion) {
|
||||
// With R=I, identical K, and zero distortion the rectification maps are
|
||||
// near-identity. The mean absolute difference between input and output
|
||||
// should be small (< 5 intensity levels out of 255).
|
||||
score::StereoRectifier rectifier(make_calib());
|
||||
|
||||
// Deterministic gradient: pixel value = (row + col) % 256
|
||||
cv::Mat left(kHeight, kWidth, CV_8UC1);
|
||||
for (int r = 0; r < kHeight; ++r)
|
||||
for (int c = 0; c < kWidth; ++c)
|
||||
left.at<uchar>(r, c) = static_cast<uchar>((r + c) % 256);
|
||||
cv::Mat right = left.clone();
|
||||
|
||||
auto [rl, rr] = rectifier.rectify(left, right);
|
||||
|
||||
// Compare rectified-left row sums to original row sums
|
||||
cv::Mat diff;
|
||||
cv::absdiff(rl, left, diff);
|
||||
const double mean_diff = cv::mean(diff)[0];
|
||||
EXPECT_LT(mean_diff, 5.0)
|
||||
<< "Mean absolute pixel difference after rectification is " << mean_diff
|
||||
<< " — expected near-identity for zero-distortion identical cameras";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Invalid calibration → throws
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(StereoRectifierTest, InvalidKSizeThrows) {
|
||||
auto calib = make_calib();
|
||||
calib.k_left = cv::Mat::eye(2, 3, CV_64F); // wrong: 2x3 instead of 3x3
|
||||
EXPECT_THROW(score::StereoRectifier{calib}, std::invalid_argument);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, InvalidKTypeThrows) {
|
||||
auto calib = make_calib();
|
||||
calib.k_left = cv::Mat::eye(3, 3, CV_32F); // wrong type
|
||||
EXPECT_THROW(score::StereoRectifier{calib}, std::invalid_argument);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, InvalidDistSizeThrows) {
|
||||
auto calib = make_calib();
|
||||
calib.d_right = cv::Mat::zeros(1, 4, CV_64F); // 1x4, needs 1x5
|
||||
EXPECT_THROW(score::StereoRectifier{calib}, std::invalid_argument);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, InvalidRotationSizeThrows) {
|
||||
auto calib = make_calib();
|
||||
calib.r = cv::Mat::eye(3, 2, CV_64F); // 3x2, needs 3x3
|
||||
EXPECT_THROW(score::StereoRectifier{calib}, std::invalid_argument);
|
||||
}
|
||||
|
||||
TEST(StereoRectifierTest, InvalidTranslationSizeThrows) {
|
||||
auto calib = make_calib();
|
||||
calib.t = cv::Mat::zeros(1, 3, CV_64F); // 1x3, needs 3x1
|
||||
EXPECT_THROW(score::StereoRectifier{calib}, std::invalid_argument);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user