3D Geometry Structure and Algorithm

3D Geometry Structure

class IGeo3d

Common abstract base class for all 3D geometry types.

Subclassed by visionflow::geo3d::Line3f, visionflow::geo3d::Plane3f, visionflow::geo3d::Point3f, visionflow::geo3d::Vector3f

Public Types

enum Geo3dType

3D geometry enum value.

Values:

enumerator kPoint3f = 0

enumerator kVector3f = 1

enumerator kLine3f = 2

enumerator kPlane3f = 3

Public Functions

virtual IGeo3d::Geo3dType type() const = 0

Enumerate value of geometry types.

Returns:

Geo3dType

const char *type_name() const

Name of geometry types.

Returns:

const char*

inline virtual bool is_solid() const

Get if the geometry object is a solid.

Returns:

True if the geometry object is a solid. else false.

struct Point3f : public visionflow::geo3d::IGeo3d

Floating point 3D coordinate points.

Public Functions

Point3f() = default

< Default constructor,no initialization.

Constructor with three values.

Point3f(float x, float y, float z)

Constructor with a Vector.

bool operator==(const Vector3f &rhs) const

Numerical comparison with vectors of the same type.

Translation of points

Point3f operator+(const Vector3f &rhs) const

Translation of points.

Point3f operator-(const Vector3f &rhs) const

Enlarge point coordinates.

Point3f operator*(float rhs) const

Point coordinates reduced.

Point3f operator/(float rhs) const

Translation of points.

Point3f &operator+=(const Vector3f &rhs)

Translation of points.

Point3f &operator-=(const Vector3f &rhs)

Enlarge point coordinates.

Point3f &operator*=(float rhs)

Point coordinates reduced.

Point3f &operator/=(float rhs)

dot product

virtual IGeo3d::Geo3dType type() const override

x coordinate

Public Members

float x = 0

y coordinate

float y = 0

z coordinate

struct Vector3f : public visionflow::geo3d::IGeo3d

A floating-point 3d vector with size and direction from the origin point (0, 0, 0).

Public Functions

Vector3f() = default

< Default constructor,no initialization.

Constructor with three values.

Vector3f(float x, float y, float z)

Constructor with a Point.

bool operator==(const Vector3f &rhs) const

Inversion of vectors.

Vector3f operator-() const

Addition of vectors.

Vector3f operator+(const Vector3f &rhs) const

Subtraction of vectors.

Vector3f operator-(const Vector3f &rhs) const

Amplification of vectors.

Vector3f operator*(float rhs) const

Reduction of vectors.

Vector3f operator/(float rhs) const

vector dot product

float operator*(const Vector3f &rhs) const

vector cross product

Vector3f operator^(const Vector3f &rhs) const

Addition of vectors.

Vector3f &operator+=(const Vector3f &rhs)

Subtraction of vectors.

Vector3f &operator-=(const Vector3f &rhs)

Amplification of vectors.

Vector3f &operator*=(float rhs)

Reduction of vectors.

virtual IGeo3d::Geo3dType type() const override

Enumerate value of geometry types.

Returns:

Geo3dType

Vector3f normalize() const

Normalized vector.

Returns:

Vector3f Normalized vector.

float length() const

Norm of a vector.

Returns:

float Norm length.

bool is_parallel_to(const Vector3f &vec) const

Verify whether two vectors are parallel.

struct Plane3f : public visionflow::geo3d::IGeo3d

3D float point plane.

Public Functions

Plane3f() = default

< Default constructor,no initialization.

Plane3f(float a, float b, float c, float d)

Constructor with ax+by+cz+d=0.

Plane3f(const Point3f &lhs, const Vector3f &rhs)

Constructor with a Point on the Plane and the normal Vector (a vector perpendicular to the plane) of the Plane.

Plane3f(const Point3f &lhs, const Point3f &mhs, const Point3f &rhs)

Constructor with three Points.

Plane3f(float x_intercept, float y_intercept, float z_intercept)

Constructor with x/x_intercept+y/y_intercept+z/z_intercept=1.

virtual IGeo3d::Geo3dType type() const override

Enumerate value of geometry types.

Returns:

Geo3dType

Vector3f norm() const

plane’s normal.

visionflow::geometry::Radian angle_x_normal_project() const

the angle between x positive axis and vector projected by normal vector on xy plane, the range is (-𝝅, 𝝅].

visionflow::geometry::Radian angle_z_normal() const

angle between normal and z axis, the range is [0, 𝝅].

Point3f perpendicular_foot(const Point3f &point) const

perpendicular foot from point to plane

float distance_along_x_axis(const Point3f &point) const

The distance from the point to the plane along x axis.

Parameters:

point – the point to calculate distance.

Returns:

Distance from the point to the plane along x axis, note that the distance is infinity if plane is parallel to the x axis.

float distance_along_y_axis(const Point3f &point) const

The distance from the point to the plane along y axis.

Parameters:

point – the point to calculate distance.

Returns:

Distance from the point to the plane along y axis, note that the distance is infinity if plane is parallel to the y axis.

float distance_along_z_axis(const Point3f &point) const

The distance from the point to the plane along z axis.

Parameters:

point – the point to calculate distance.

Returns:

Distance from the point to the plane along z axis, note that the distance is infinity if plane is parallel to the z axis.

struct Line3f : public visionflow::geo3d::IGeo3d

3D float point straight line.

Public Functions

Line3f() = default

< Default constructor,no initialization.

Line3f(const Plane3f &lhs, const Plane3f &rhs)

Constructor with two Planes.

Line3f(const Point3f &lhs, const Point3f &rhs)

Constructor with two Points.

Line3f(Point3f lhs, const Vector3f &rhs)

Constructor with a Point on the Line and the direction Vector of the Line.

virtual IGeo3d::Geo3dType type() const override

Enumerate value of geometry types.

Returns:

Geo3dType

Point3f perpendicular_foot(const Point3f &point) const

Perpendicular foot of given point to the line.

Parameters:

point – The point to calculate perpendicular foot.

Returns:

Perpendicular foot point. The point one the line.

Public Members

Point3f point

The normalized direction vector on the line, note that its length is 1.

class Matrix4f

4x4 float matrix.

Transformation matrix.

Public Functions

explicit Matrix4f(float diag = 1)

Construct a 4*4 float matrix.

The value of the principal diagonal is diag, the rest are 0.

Parameters:

diag – The principal diagonal value. If rhs is 1(As default), it will construct a identity matrix.

Matrix4f(const std::vector<float> &row1, const std::vector<float> &row2, const std::vector<float> &row3, const std::vector<float> &row4)

Construct a new Matrix4f object.

Parameters:

• row1 – The first row of matrix

• row2 – The second row of matrix

• row3 – The third row of matrix

• row4 – The fourth row of matrix

Throws:

visionflow::excepts::InvalidArgument – If the input matrix size is not 4*4.

Matrix4f(float mat_0_0, float mat_0_1, float mat_0_2, float mat_0_3, float mat_1_0, float mat_1_1, float mat_1_2, float mat_1_3, float mat_2_0, float mat_2_1, float mat_2_2, float mat_2_3, float mat_3_0, float mat_3_1, float mat_3_2, float mat_3_3)

Construct a 4*4 float matrix.

Read parameters in first row the col order.

Matrix4f(const Matrix4f &rhs)

Deep copy constructor.

It will not share the same references as the source object.

Parameters:

rhs – other Matrix4f.

Matrix4f(Matrix4f &&rhs) noexcept

Move constructor.

Parameters:

rhs – other rvalue Matrix4f.

Matrix4f operator*(const Matrix4f &rhs) const

Matrix multiplication.

A * B = C.

Parameters:

rhs – B matrix.

Returns:

Matrix4f C matrix.

Matrix4f transpose() const

Return the transposed matrix.

Returns:

Matrix4f transposed matrix.

float at(size_t row, size_t col) const

Return the specified array element.

if row or col out of range, an visionflow::excepts::InvalidArgument will be thrown.

Parameters:

• row – Index along the dimension 1.

• col – Index along the dimension 2.

Throws:

visionflow::excepts::InvalidArgument – if row or col out of range.

Returns:

NO_DISCARD float array element.

float &at(size_t row, size_t col)

Return a reference to the specified array element.

if row or col out of range, an visionflow::excepts::InvalidArgument will be thrown.

Parameters:

• row – Index along the dimension 1.

• col – Index along the dimension 2.

Throws:

visionflow::excepts::InvalidArgument – if row or col out of range.

Returns:

NO_DISCARD float& array element.

Public Static Attributes

static constexpr size_t kSide = 4

The matrix row and col number.

3D Geometry algorithms

group distance_geo3d

Calculate the distance between two 3D geometry shapes.

shapes.

param lhs:

[in] geometry shape

param rhs:

[in] geometry shape

param result:

[out] The distance between geo1 and geo2

Functions

float distance(const visionflow::geo3d::Point3f &lhs, const visionflow::geo3d::Point3f &rhs)

float distance(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Line3f &rhs)

float distance(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Plane3f &rhs)

float distance(const visionflow::geo3d::Point3f &lhs, const visionflow::geo3d::Line3f &rhs)

float distance(const visionflow::geo3d::Point3f &lhs, const visionflow::geo3d::Plane3f &rhs)

float distance(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Plane3f &rhs)

float distance(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Point3f &rhs)

float distance(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Point3f &rhs)

float distance(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Line3f &rhs)

group intersection_geo3d

Calculate the intersection between the input 3D geometry shapes.

the input 3D geometry shapes.

throws excepts::InvalidArgument:

If the input 3D geometry shapes do not intersect.

param lhs:

[in] geometry shape

param rhs:

[in] geometry shape

param result:

[out] The intersection of geo1 and geo2

Functions

visionflow::geo3d::Point3f intersection(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Line3f &rhs)

visionflow::geo3d::Line3f intersection(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Plane3f &rhs)

group intersection_point

Calculate the intersection point between the input 3D geometry shapes.

the input 3D geometry shapes.

throws excepts::InvalidArgument:

If the input 3D geometry shapes do not have an intersection point.

param lhs:

[in] geometry shape

param rhs:

[in] geometry shape

param result:

[out] The intersection point of geo1 and geo2

Functions

visionflow::geo3d::Point3f intersection_point(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Plane3f &rhs)

visionflow::geo3d::Point3f intersection_point(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Line3f &rhs)

visionflow::geo3d::Point3f intersection_point(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Plane3f &mhs, const visionflow::geo3d::Plane3f &rhs)

group intersects_one_point_geo3d

Verify if two 3D geometry shape have only one intersection point.

shapes have only one intersection point.

param lhs:

[in] geometry shape

param rhs:

[in] geometry shape

Functions

bool intersects_one_point(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Plane3f &rhs)

bool intersects_one_point(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Line3f &rhs)

bool intersects_one_point(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Plane3f &mhs, const visionflow::geo3d::Plane3f &rhs)

group intersects_geo3d

Verify if two 3D geometry shape intersect.

shapes intersect (at least one intersection point).

param lhs:

[in] geometry shape

param rhs:

[in] geometry shape

Functions

bool intersects(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Line3f &rhs)

bool intersects(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Plane3f &rhs)

group min_angle_geo3d

Calculate the minimum angle between two 3D geometry shapes.

geometry shapes.

param lhs:

geometry shape

param rhs:

geometry shape

return:

The acute minimum angle radian of two 3D geometry shapes, the range is [0, 𝝅/2]

Functions

visionflow::geometry::Radian min_angle(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Line3f &rhs)

visionflow::geometry::Radian min_angle(const visionflow::geo3d::Line3f &lhs, const visionflow::geo3d::Plane3f &rhs)

visionflow::geometry::Radian min_angle(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Line3f &rhs)

visionflow::geometry::Radian min_angle(const visionflow::geo3d::Plane3f &lhs, const visionflow::geo3d::Plane3f &rhs)

group transform_rotate_geo3d

3D geometry shape rotation around x/y/z axes centered at a given point (Defalt: origin of the coordinate system).

The coordinate system follows the right-hand rule: the thumb points along the axis’s positive direction, and the curled fingers indicate the positive rotation direction. If Euler angles are used to represent the rotation process with intrinsic rotations, and the rotation order is x-y-z (where R(x), R(y), and R(z) represent the rotation matrices around the x, y, and z axes, respectively), then the final rotation matrix is \( R(z) * R(y) * *R(x) \). The R(x) matrix is:

\[\begin{split} \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos\theta_x & -sin\theta_x \\ 0 & sin\theta_x & cos\theta_x \\ \end{bmatrix} \end{split}\]

The R(y) matrix is:

\[\begin{split} \begin{bmatrix} cos\theta_y & 0 & sin\theta_y \\ 0 & 1 & 0 \\ -sin\theta_y & 0 & cos\theta_y \\ \end{bmatrix} \end{split}\]

The R(z) matrix is:

\[\begin{split} \begin{bmatrix} cos\theta_z & -sin\theta_z & 0 \\ sin\theta_z & cos\theta_z & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \end{split}\]

param geometry:

Geometry.

Geometry
Point
Vector
Line
Plane

param theta_x:

X-axis rotation radian.

param theta_y:

Y-axis rotation radian.

param theta_z:

Z-axis rotation radian.

param center:

The rotate center.

return:

The rotated 3d geometry.

Functions

visionflow::geo3d::Point3f transform_rotate(const visionflow::geo3d::Point3f &geometry, const visionflow::geometry::Radian &theta_x, const visionflow::geometry::Radian &theta_y, const visionflow::geometry::Radian &theta_z, const visionflow::geo3d::Point3f &center = {0, 0, 0})

visionflow::geo3d::Vector3f transform_rotate(const visionflow::geo3d::Vector3f &geometry, const visionflow::geometry::Radian &theta_x, const visionflow::geometry::Radian &theta_y, const visionflow::geometry::Radian &theta_z, const visionflow::geo3d::Point3f &center = {0, 0, 0})

visionflow::geo3d::Line3f transform_rotate(const visionflow::geo3d::Line3f &geometry, const visionflow::geometry::Radian &theta_x, const visionflow::geometry::Radian &theta_y, const visionflow::geometry::Radian &theta_z, const visionflow::geo3d::Point3f &center = {0, 0, 0})

visionflow::geo3d::Plane3f transform_rotate(const visionflow::geo3d::Plane3f &geometry, const visionflow::geometry::Radian &theta_x, const visionflow::geometry::Radian &theta_y, const visionflow::geometry::Radian &theta_z, const visionflow::geo3d::Point3f &center = {0, 0, 0})

group transform_translate_geo3d

3D geometry translation along x/y/z axes.

param geometry:

Geometry.

Geometry
Point
Vector
Line
Plane

param offset_x:

Translation transform direction-x.

param offset_y:

Translation transform direction-y.

param offset_z:

Translation transform direction-z.

return:

The translated 3d geometry.

Functions

visionflow::geo3d::Point3f transform_translate(const visionflow::geo3d::Point3f &geometry, float offset_x, float offset_y, float offset_z)

visionflow::geo3d::Vector3f transform_translate(const visionflow::geo3d::Vector3f &geometry, float offset_x, float offset_y, float offset_z)

visionflow::geo3d::Line3f transform_translate(const visionflow::geo3d::Line3f &geometry, float offset_x, float offset_y, float offset_z)

visionflow::geo3d::Plane3f transform_translate(const visionflow::geo3d::Plane3f &geometry, float offset_x, float offset_y, float offset_z)

group transform_geo3d

3D geometry transform.

transform(geometry, step1), transform(geometry, step2) -> transform(geometry, step2 * step1).

param geometry:

Geometry.

Geometry
Point
Vector
Line
Plane

param mat:

Transformation matrix.

return:

the transformed 3d geometry.

Functions

visionflow::geo3d::Point3f transform(const visionflow::geo3d::Point3f &geometry, const visionflow::geo3d::Matrix4f &mat)

visionflow::geo3d::Vector3f transform(const visionflow::geo3d::Vector3f &geometry, const visionflow::geo3d::Matrix4f &mat)

visionflow::geo3d::Line3f transform(const visionflow::geo3d::Line3f &geometry, const visionflow::geo3d::Matrix4f &mat)

visionflow::geo3d::Plane3f transform(const visionflow::geo3d::Plane3f &geometry, const visionflow::geo3d::Matrix4f &mat)

Matrix4f visionflow::geo3d::get_affine_transform(const std::vector<Point3f> &src, const std::vector<Point3f> &dst, bool have_noise_point = false, float threshold = 0.1, size_t iterations = 10)

Compute the affine transformation matrix from the point pairs consisting of the input source point set and destination point set.

If the number of point pairs is less than 4, an visionflow::excepts::InvalidArgument will be thrown.

The internal computational process is as follows:

1. Pair the input source point set and destination point set sequentially.

2. If the number of input points is 4, or if have_noise_point is false, the internal process uses the least squares method to estimate the affine matrix. This method is suitable for scenarios without outliers. After obtaining the affine matrix, it returns. Otherwise, proceed to step 3.

3. Check if the input max_iteration is less than 1. If so, recalculate max_iteration using an adaptive method. Otherwise, no action is taken.

4. Iterate based on the parameter max_iteration: 4-1. Randomly select 5 non-collinear points to estimate the affine matrix. 4-2. Using the estimated affine matrix and the source point set, compute the transformed point set. Calculate the Euclidean distance error between the transformed point set and the destination point set. If the error is greater than the input parameter threshold, the current point is considered an outlier; otherwise, it is considered an inlier. Record the number of inliers. 4-3. If the previously input max_iteration was invalid and an adaptive method was used to recalculate it, the number of max_iteration needs to be updated.

5. Record the affine transformation matrix with the highest number of inliers. Re-estimate the affine matrix based on the inliers from this iteration and return it.

Parameters:

• src – Source point set (at least 4 non-coplanar 3D points).

• dst – Destination point set (at least 4 non-coplanar 3D points).

• have_noise_point – If false, the affine matrix is estimated using the least squares method, which is suitable for scenarios without outliers; if true, the affine matrix is estimated using the RANSAC method. It effectively removes mismatched points (outliers) and handles noise, partial occlusion, and matching errors but is more time-consuming than least squares.

• threshold – This parameter is effective when have_noise_point=true, and serves as the input parameter “threshold” in the calculation process described in step 4-2 above, used to determine inliers and outliers. Typically configured as 2 to 3 times the 3D camera’s imaging error.​

• iterations – This parameter takes effect when have_noise_point=true, corresponding to the “max_iteration” in steps 3 and 4 of the computational process described above. When estimating the number of iterations, assuming the number of input points is \( n \), and considering the worst-case scenario where the outlier ratio is close to 50% (inlier ratio \( w = 0.5 \)) with a confidence level set to \( p = 0.99 \),and since the RANSAC iteration calculation formula is \( k = \left\lceil \frac{\log(1-p)}{\log(1-w^n)} \right\rceil \), the final formula for determining the number of iterations is: \( k = \left\lceil \frac{\log(0.01)}{\log(1-0.5^n)} \right\rceil \).

Throws:

visionflow::excepts::InvalidArgument – if the number of point pairs is less than requirement.

Returns:

transformation matrix (which includes only translation, rotation, and scaling, therefore the last row of the matrix is always [0, 0, 0, 1]).

Matrix4f visionflow::geo3d::get_inverse_transform(const Matrix4f &mat)

Get inverse transform matrix.

Parameters:

mat – Source matrix.

Returns:

inverse transform matrix.

Matrix4f visionflow::geo3d::get_rotate_transform(const geometry::Radian &theta_x, const geometry::Radian &theta_y, const geometry::Radian &theta_z, const Point3f &center = {0, 0, 0})

Get 3D rotate transform matrix.

The coordinate system follows the right-hand rule, where the thumb points in the positive axis direction and the curled fingers indicate the positive rotation direction. The transformation matrix is estimated with the given point as the rotation center, following the rotation order x-y-z. Let the center pos be \( (x_c, y_c, z_c) \), the matrix is:

\[\begin{split} \begin{bmatrix} \cos\theta_y \cos\theta_z & \sin\theta_x \sin\theta_y \cos\theta_z - \cos\theta_x \sin\theta_z & \cos\theta_x \sin\theta_y \cos\theta_z + \sin\theta_x \sin\theta_z & x_c (1 - \cos\theta_y \cos\theta_z) + y_c (\cos\theta_x \sin\theta_z - \sin\theta_x \sin\theta_y \cos\theta_z) + z_c (-\cos\theta_x \sin\theta_y \cos\theta_z - \sin\theta_x \sin\theta_z) \\ \cos\theta_y \sin\theta_z & \sin\theta_x \sin\theta_y \sin\theta_z + \cos\theta_x \cos\theta_z & \cos\theta_x \sin\theta_y \sin\theta_z - \sin\theta_x \cos\theta_z & y_c (1 - \cos\theta_x \cos\theta_z - \sin\theta_x \sin\theta_y \sin\theta_z) + x_c (-\cos\theta_y \sin\theta_z) + z_c (\sin\theta_x \cos\theta_z - \cos\theta_x \sin\theta_y \sin\theta_z) \\ -\sin\theta_y & \sin\theta_x \cos\theta_y & \cos\theta_x \cos\theta_y & z_c (1 - \cos\theta_x \cos\theta_y) + x_c \sin\theta_y - y_c \sin\theta_x \cos\theta_y \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \end{split}\]

Parameters:

• theta_x – X-axis rotation radian.

• theta_y – Y-axis rotation radian.

• theta_z – Z-axis rotation radian.

• center – The rotate center.

Returns:

Matrix4f rotate transform matrix.

Matrix4f visionflow::geo3d::get_scale_transform(float s_x, float s_y, float s_z, const Point3f &center = {0, 0, 0})

Get the transformation matrix scaled to the given point.

Let the center pos be \( (x_c, y_c) \), then the transform matrix is:

\[\begin{split} \begin{bmatrix} s_x & 0 & 0 & (1 - s_x) x_c \\ 0 & s_y & 0 & (1 - s_y) y_c \\ 0 & 0 & s_z & (1 - s_z) z_c \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{split}\]

Parameters:

• s_x – Scaling ratio direction-x.

• s_y – Scaling ratio direction-y.

• s_z – Scaling ratio direction-z.

• center – The scale center (default is the coordinate system origin).

Returns:

Matrix4f scale transform matrix.

Matrix4f visionflow::geo3d::get_translate_transform(float offset_x, float offset_y, float offset_z)

Get translate transform matrix.

The translations along the x, y, z axes are represented by \( t_x, t_y, t_z \), then the transform matrix is：

\[\begin{split} \begin{bmatrix} 0 & 0 & 0 & t_x \\ 0 & 0 & 0 & t_y \\ 0 & 0 & 0 & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{split}\]

Parameters:

• offset_x – Translation transform direction-x.

• offset_y – Translation transform direction-y.

• offset_z – Translation transform direction-z.

Returns:

translate transform matrix