stereoutil.hpp

namespace stereo

Functions

Geometry compute_stereo_geometry(const Calib &calib)

Computes all stereo geometry matrices from calibration parameters.

Builds the intrinsic matrices of both cameras, the rotation and translation between them (in skew-symmetric form), and the resulting fundamental matrix.

Parameters:

calib – Stereo calibration parameters (intrinsics, rotation, translation).

Returns:

Geometry struct containing K0, K1, R, t_x, and F.

void undistortPoint(double &x_undistorted, double &y_undistorted, const double x_distorted, const double y_distorted, const Eigen::Matrix3d &K, const std::vector<double> &d)
Eigen::Matrix3d rotation_from_euler(const std::vector<double> &rot_deg)

Computes a rotation matrix from Euler angles (degrees, XYZ order).

Parameters:

rot_deg – Euler angles in degrees: [theta_x, phi_y, psi_z].

Returns:

3x3 rotation matrix Rz * Ry * Rx.

Eigen::Matrix3d camera_matrix(const CamIntrinsics &cam)

Constructs a camera intrinsic matrix from calibration parameters.

Parameters:

cam – Camera intrinsics (fx, fy, fs, cx, cy).

Returns:

3x3 intrinsic matrix K.

Eigen::Matrix3d skew_translation(const std::vector<double> &t)

Forms the skew-symmetric matrix of a translation vector.

Parameters:

t – Translation vector [tx, ty, tz].

Returns:

3x3 skew-symmetric matrix t_x.

Eigen::MatrixXd patch_corners(const double x, const double y, const double size_x, const double size_y)

Computes the homogeneous corner coordinates of an image patch.

Parameters:

  • x – Top-left x coordinate.

  • y – Top-left y coordinate.

  • size_x – Patch width.

  • size_y – Patch height.

Returns:

3x4 matrix of patch corner points.

Eigen::Matrix3d fundamental(const Eigen::Matrix3d &K0, const Eigen::Matrix3d &K1, const Eigen::Matrix3d &t_x, const Eigen::Matrix3d &R)

Computes the fundamental matrix from intrinsics, translation, and rotation.

Parameters:

  • K0 – Intrinsic matrix of camera 0.

  • K1 – Intrinsic matrix of camera 1.

  • t_x – Skew-symmetric translation matrix.

  • R – Rotation from camera 0 to camera 1.

Returns:

Normalized 3x3 fundamental matrix F.

EpipolarStrip calc_epi_strip_points(const double cx, const double cy, const Eigen::Matrix<double, 3, 4> &lines, const Eigen::Matrix3d F, double range)

Calculates epipolar strip geometry around a patch centre.

Parameters:

  • cx – Patch centre x coordinate.

  • cy – Patch centre y coordinate.

  • lines – Bounding box lines in homogeneous form.

  • F – Fundamental matrix.

  • range – Half-length of the epipolar search segment.

Returns:

EpipolarStrip containing midpoint and bounding quad points.

std::tuple<int, int, int, int> bounding_box(const EpipolarStrip &strip, const int px_hori, const int px_vert)

Computes the image-space bounding box of an epipolar strip.

Parameters:

  • strip – Epipolar strip quad points.

  • px_hori – Image width in pixels.

  • px_vert – Image height in pixels.

Returns:

(xmin, xmax, ymin, ymax) bounding coordinates.

void pixel_to_world(const subset::Grid &ss_grid, const Calib &calib, ResultArrays &temporal, ResultArrays &stereo_ref, ResultArrays &stereo_def, const Eigen::Matrix3d &K0, const Eigen::Matrix3d &K1, const Eigen::Matrix3d &R, const int ss_size, const bool first_frame = false)

Triangulates 3D world coordinates for valid stereo matches (linear DLT).

For each subset center whose match passes the threshold, this function: 1) Computes the left/right patch center in pixels, 2) Undistorts both points using each camera’s intrinsics/distortion, 3) Forms normalized homogeneous points (assumes K = I in projection), 4) Builds the DLT system with P0 = [I | 0], P1 = [R | t], 5) Solves via SVD and recovers X in homogeneous coordinates.

Note

Points are undistorted and treated as normalized image coordinates when building the DLT system (i.e., P0 = [I|0], P1 = [R|t]). Ensure undistortion produces normalized coordinates consistent with this assumption.

Note

Translation vector calib.translation is assumed to be in millimeters, so the resulting (X, Y, Z) will also be in millimeters.

Warning

This implementation computes X but does not store it; add assignments to persist (X_mm, Y_mm, Z_mm) into your results container as needed. TODO: NEED TO FIX THIS

Parameters:

  • ss_grid – Grid of subset (patch) top-left coordinates (pixel space).

  • calib – Calibration containing intrinsics/distortion for cam0/cam1 and translation (t).

  • stereo_matches – Match results; uses u,v (pixel disparities) and above_thresh mask.

  • K0 – Intrinsic matrix of camera 0 (used only for undistortion here).

  • K1 – Intrinsic matrix of camera 1 (used only for undistortion here).

  • R – Rotation from camera 0 to camera 1.

  • ss_size – Subset (patch) size in pixels (assumed square).

Pre:

ss_grid.num == stereo_matches.u.size() == stereo_matches.v.size() == stereo_matches.above_thresh.size()

void search_epi_line(double &best_zncc, double &best_disp_x, double &best_disp_y, const double x, const double y, const subset::Pixels &ss_l, subset::Pixels &ss_r, const Eigen::Vector2d P, const Eigen::Vector2d dir, const Interpolator &interp_r, const int range)

Searches along an epipolar line for the best ZNCC match.

Parameters:

  • best_zncc – Output: highest ZNCC score found.

  • best_disp_x – Output: best displacement in x.

  • best_disp_y – Output: best displacement in y.

  • x – Reference patch centre x.

  • y – Reference patch centre y.

  • ss_l – Reference subset (left image).

  • ss_r – Workspace subset (right image).

  • P – Midpoint on the epipolar line.

  • dir – Direction of the epipolar line (unit length).

  • interp_r – Interpolator for the right image.

  • range – Search extent in pixels along the line.

void compute_epi(Eigen::Vector2d &nearest_point, Eigen::Vector2d &direction, const double x, const double y, const Eigen::Matrix3d &F)

Computes the closest point and direction of the epipolar line for a pixel.

Parameters:

  • nearest_point – Output: orthogonal projection of (x,y) onto the epipolar line.

  • direction – Output: unit direction vector of the epipolar line.

  • x – Pixel x coordinate.

  • y – Pixel y coordinate.

  • F – Fundamental matrix.

void get_rigid_translation_from_rectified_fft(std::vector<double> &p, const double cx, const double cy, const int ss_size_x, const int ss_size_y, const int window_size_x, const int window_size_y, const Eigen::Matrix3d &F, const Interpolator &interp_ref, const Interpolator &interp_def, const double offset_x = 0.0, const double offset_y = 0.0, const bool print = false)

Estimates rigid translation using FFT correlation on a rectified search grid.

Parameters:

  • p – Output: 6‑parameter rigid/affine displacement seed.

  • cx – Subset centre x coordinate.

  • cy – Subset centre y coordinate.

  • ss_size_x – Subset width.

  • ss_size_y – Subset height.

  • closest_point – Epipolar closest point to the reference pixel.

  • dir – Epipolar direction unit vector.

  • window_size_x – FFT window width.

  • window_size_y – FFT window height.

  • img_ref – Pointer to reference image buffer.

  • interp_def – Interpolator for the deformed image.

  • offset_x – Offset from reference left subset centre to current left centre.

  • offset_y – Offset from reference left subset centre to current left centre.

void get_rigid_translation_from_rectified_search(std::vector<double> &p, const int ss_x, const int ss_y, const int ss_size_x, const int ss_size_y, const Eigen::Vector2d closest_point, const Eigen::Vector2d dir, subset::Pixels &ss_l, const Interpolator &interp_ref, const Interpolator &interp_def)

Estimates rigid translation by brute‑force ZNCC search along the epipolar line.

Parameters:

  • p – Output: 6‑parameter rigid/affine displacement seed.

  • ss_x – Subset top‑left x coordinate.

  • ss_y – Subset top‑left y coordinate.

  • ss_size_x – Subset width.

  • ss_size_y – Subset height.

  • closest_point – Epipolar closest point to the reference pixel.

  • dir – Epipolar direction unit vector.

  • ss_l – Reference subset (left image).

  • interp_ref – Interpolator for reference image.

  • interp_def – Interpolator for deformed image.

bool *compute_roi_r(const subset::Grid ss_grid, const ResultArrays &stereo_matches, const int px_hori, const int px_vert, const int ss_size_x, const int ss_size_y)
bool *compute_roi_r_test(const bool *img_roi_l, const subset::Grid &ss_grid, const ResultArrays &stereo_matches, const int px_hori, const int px_vert)
void remove_unmatched_subsets(subset::Grid &ss_grid_l, subset::Grid &ss_grid_r, const ResultArrays stereo_matches)
std::pair<std::vector<std::string>, std::vector<std::string>> split_basenames(const util::Config &conf)
struct EpipolarStrip
#include <stereoutil.hpp>

Represents the epipolar strip bounding geometry for a patch.

Stores the projected patch centre on the epipolar line and the four corner points of the bounding quadrilateral that limits the valid search region around that line.

Public Members

Eigen::Vector2d P

Patch centre projected onto the epipolar line.

Eigen::Vector2d Q0

Bounding corner Q0.

Eigen::Vector2d Q1

Bounding corner Q1.

Eigen::Vector2d Q2

Bounding corner Q2.

Eigen::Vector2d Q3

Bounding corner Q3.

struct Geometry
#include <stereoutil.hpp>

Container for stereo camera geometry derived from calibration.

Holds intrinsic matrices for both cameras, the rotation and translation between them (in skew-symmetric form), and the resulting fundamental matrix.

Public Members

Eigen::Matrix3d K0

Intrinsic matrix of camera 0.

Eigen::Matrix3d K1

Intrinsic matrix of camera 1.

Eigen::Matrix3d R

Rotation from camera 0 to camera 1.

Eigen::Matrix3d t_x

Skew-symmetric translation matrix.

Eigen::Matrix3d F

Fundamental matrix.