|
UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
|
Represents an incremental surface element (surfel) that tracks the sufficient statistics of a 3D point set for online surface estimation. More...
#include <lib/core/include/ufo/core/surfel.hpp>
Public Member Functions | |
| constexpr | Surfel ()=default |
| Default-constructs an empty surfel with no points. | |
| template<std::input_iterator InputIt> requires std::constructible_from<Vec<3, float>, std::iter_value_t<InputIt>> | |
| constexpr | Surfel (InputIt first, InputIt last) |
Constructs a surfel by adding all points in [first, last). | |
| template<std::ranges::input_range Points> requires std::constructible_from<Vec<3, float>, std::ranges::range_value_t<Points>> | |
| constexpr | Surfel (Points const &points) |
Constructs a surfel by adding all points in points. | |
| constexpr | Surfel (std::initializer_list< Vec< 3, float > > points) |
Constructs a surfel from a brace-enclosed list of Vec3f points. | |
| constexpr | Surfel (Vec< 3, float > position, Mat< 3, 3, float > scatter={}) |
| Constructs a surfel from a single position and optional scatter matrix. | |
| constexpr | Surfel (Vec< 3, float > sum, std::array< float, 6 > sum_squares, std::uint32_t num_points) |
| Constructs a surfel directly from its raw sufficient statistics. | |
| template<std::input_iterator InputIt> requires std::constructible_from<Vec<3, float>, std::iter_value_t<InputIt>> | |
| constexpr void | add (InputIt first, InputIt last) |
Adds all points in [first, last) using a batch algorithm. | |
| template<std::ranges::input_range Points> requires std::constructible_from<Vec<3, float>, std::ranges::range_value_t<Points>> | |
| constexpr void | add (Points const &points) |
Adds all points in points using the batch algorithm. | |
| constexpr void | add (std::initializer_list< Vec< 3, float > > points) noexcept |
| Adds points from a brace-enclosed initializer list. | |
| constexpr void | add (Surfel const &surfel) noexcept |
| Merges another surfel into this one using a numerically stable parallel update rule. | |
| constexpr void | add (Vec< 3, float > point) noexcept |
| Adds a single 3D point using an online (Welford-like) update rule. | |
| constexpr void | clear () noexcept |
| Clears the surfel to an empty state (no points, zero statistics). | |
| constexpr Mat< 3, 3, double > | covariance () const noexcept |
| Returns the 3×3 sample covariance matrix. | |
| constexpr bool | empty () const noexcept |
| Returns true if no points have been added. | |
| constexpr Vec< 3, double > | mean () const noexcept |
| Returns the centroid (mean position) of all accumulated points. | |
| constexpr Vec3d | normal () const noexcept |
| Returns the surface normal as the eigenvector of the covariance matrix corresponding to the smallest eigenvalue. | |
| constexpr std::uint32_t | numPoints () const noexcept |
| Returns the number of points currently represented by this surfel. | |
| constexpr Surfel & | operator+= (Surfel const &rhs) noexcept |
| Merges another surfel into this one using a numerically stable parallel update rule. | |
| constexpr Surfel & | operator+= (Vec< 3, float > rhs) noexcept |
| Adds a single 3D point to this surfel using an online (Welford-like) update rule. | |
| constexpr Surfel & | operator-= (Surfel const &rhs) noexcept |
| Removes another surfel's contribution from this one using a numerically stable parallel update rule. | |
| constexpr Surfel & | operator-= (Vec< 3, float > rhs) noexcept |
Removes a single 3D point from this surfel and returns *this. | |
| constexpr bool | operator== (Surfel const &) const noexcept=default |
| Equality comparison on all three stored statistics. | |
| constexpr double | planarity () const noexcept |
| Returns a planarity measure in [0, 1]. | |
| template<std::input_iterator InputIt> requires std::constructible_from<Vec<3, float>, std::iter_value_t<InputIt>> | |
| constexpr void | remove (InputIt first, InputIt last) |
Removes all points in [first, last) using a batch algorithm. | |
| template<std::ranges::input_range Points> requires std::constructible_from<Vec<3, float>, std::ranges::range_value_t<Points>> | |
| constexpr void | remove (Points const &points) |
Removes all points in points using the batch algorithm. | |
| constexpr void | remove (std::initializer_list< Vec< 3, float > > points) noexcept |
| Removes all points in a brace-enclosed initializer list. | |
| constexpr void | remove (Surfel const &surfel) noexcept |
| Removes a previously-added surfel's contribution from this one. | |
| constexpr void | remove (Vec< 3, float > point) noexcept |
| Removes a single previously-added 3D point. | |
| constexpr Vec< 3, float > | sum () const noexcept |
| Returns the raw sum of all point positions. | |
| constexpr std::array< float, 6 > | sumSquares () const noexcept |
| Returns the raw scatter matrix upper triangle (Sxx, Sxy, Sxz, Syy, Syz, Szz). | |
Protected Member Functions | |
| constexpr void | add (std::array< double, 6 > ss, Vec< 3, double > const &sum, std::uint32_t num_points) |
| Helper to add a batch of points represented by their scatter statistics. | |
| constexpr void | remove (std::array< double, 6 > ss, Vec< 3, double > const &sum, std::uint32_t num_points) |
| Helper to remove a batch of points represented by their scatter statistics. | |
Static Protected Member Functions | |
| static constexpr void | addOuter (std::array< double, 6 > &ss, Vec< 3, double > const &v, double scale) noexcept |
| Helper to add the outer product correction term in the parallel update. | |
| template<std::input_iterator InputIt> requires std::constructible_from<Vec<3, double>, std::iter_value_t<InputIt>> | |
| static constexpr std::tuple< std::array< double, 6 >, Vec< 3, double >, std::uint32_t > | batchScatter (InputIt first, InputIt last) noexcept |
Computes scatter statistics (ss, sum, n) for a batch of points. | |
| static constexpr std::array< double, 6 > | toDouble (std::array< float, 6 > const &arr) noexcept |
Converts an array of six floats to doubles for intermediate calculations. | |
| static constexpr void | toFloat (std::array< double, 6 > const &src, std::array< float, 6 > &dst) noexcept |
Converts an array of six doubles to floats for storage. | |
Protected Attributes | |
| std::uint32_t | num_points_ {} |
| The number of contributing points. This is used to compute the mean and covariance from the raw sums. If this is zero, the surfel is considered empty and all statistics are zero. If this is one, the mean is equal to the single point and the covariance is undefined (but treated as zero). For two or more points, the mean and covariance are computed from the sums as described in the method documentation. | |
| Vec< 3, float > | sum_ {} |
The sum of all point positions (a Vec<3, float>). This is the unnormalized sum; the mean can be obtained by dividing this sum by numPoints(), or by calling the mean() method directly. | |
| std::array< float, 6 > | sum_squares_ {} |
Upper triangle of the scatter matrix S, stored as six floats in the order (Sxx, Sxy, Sxz, Syy, Syz, Szz). S accumulates the sum of squared deviations from the running mean; the sample covariance is S / (n − 1). | |
Friends | |
| Surfel | operator+ (Surfel lhs, Surfel const &rhs) noexcept |
Returns a new surfel that is the merge of lhs and rhs. | |
| Surfel | operator+ (Surfel lhs, Vec< 3, float > rhs) noexcept |
Returns a new surfel that is the result of adding point rhs to lhs. | |
| Surfel | operator- (Surfel lhs, Surfel const &rhs) noexcept |
Returns a new surfel that is the result of removing rhs from lhs. | |
| Surfel | operator- (Surfel lhs, Vec< 3, float > rhs) noexcept |
Returns a new surfel that is the result of removing point rhs from lhs. | |
Represents an incremental surface element (surfel) that tracks the sufficient statistics of a 3D point set for online surface estimation.
A Surfel maintains three quantities that together allow the mean, full 3×3 covariance matrix, eigenvalues, eigenvectors, surface normal, and planarity of an arbitrary set of 3D points to be computed at any time, without storing the individual points:
sum_ — the sum of all point positions (a Vec<3, float>).sum_squares_ — the upper triangle of the scatter matrix S, stored as six floats in the order (Sxx, Sxy, Sxz, Syy, Syz, Szz). S accumulates the sum of squared deviations from the running mean; the sample covariance is S / (n − 1).num_points_ — the number of contributing points.Points and entire surfels can be added or removed incrementally. The addition algorithm uses a numerically stable parallel / online update rule (analogous to Welford's algorithm for variance) so that the scatter matrix remains accurate as the point count grows. Batch addition uses a two-pass algorithm for improved numerical conditioning. All intermediate arithmetic is performed in double precision; results are stored as float.
Intended for use as a per-point or per-node attribute in surface reconstruction, mapping, or point cloud processing.
Derived quantities
| Method | Description |
|---|---|
mean() | Centroid of the point set. |
covariance() | 3×3 sample covariance matrix. |
normal() | Unit surface normal = eigenvector of the smallest eigenvalue. |
planarity() | Planarity measure in [0, 1]; 1 means perfectly planar. |
Definition at line 96 of file surfel.hpp.
|
inlineconstexpr |
Constructs a surfel directly from its raw sufficient statistics.
| [in] | sum | Sum of all point positions. |
| [in] | sum_squares | Upper triangle of the scatter matrix (Sxx, Sxy, Sxz, Syy, Syz, Szz). |
| [in] | num_points | Number of points represented by this surfel. |
Intended for deserialization or copying internal state.
Definition at line 119 of file surfel.hpp.
|
inlineconstexpr |
Constructs a surfel from a single position and optional scatter matrix.
| [in] | position | The point position. |
| [in] | scatter | Optional scatter matrix. |
Definition at line 130 of file surfel.hpp.
|
inlineconstexpr |
Constructs a surfel by adding all points in [first, last).
| InputIt | std::input_iterator whose value type is convertible to Vec3d. |
| [in] | first | The beginning of the input range. |
| [in] | last | The end of the input range. |
Definition at line 147 of file surfel.hpp.
|
inlineconstexpr |
Constructs a surfel by adding all points in points.
| Points | std::ranges::input_range whose elements are convertible to Vec3d. |
| [in] | points | The range of points to add. |
Definition at line 160 of file surfel.hpp.
|
inlineconstexpr |
Constructs a surfel from a brace-enclosed list of Vec3f points.
| [in] | points | The list of points to add. |
Definition at line 168 of file surfel.hpp.
|
inlineconstexpr |
Adds all points in [first, last) using a batch algorithm.
| InputIt | std::input_iterator whose value type is convertible to Vec3d. |
| [in] | first | The beginning of the input range. |
| [in] | last | The end of the input range. |
Accumulates raw second moments in a single pass, then applies a centering correction before merging with the parallel update rule.
Definition at line 297 of file surfel.hpp.
|
inlineconstexpr |
Adds all points in points using the batch algorithm.
| Points | std::ranges::input_range whose elements are convertible to Vec3d. |
| [in] | points | The input range of points. |
Definition at line 314 of file surfel.hpp.
|
inlineconstexprprotected |
Helper to add a batch of points represented by their scatter statistics.
| [in] | ss | The scatter matrix upper triangle of the batch to add. |
| [in] | sum | The sum of all point positions in the batch. |
| [in] | num_points | The number of points in the batch. |
Definition at line 716 of file surfel.hpp.
|
inlineconstexprnoexcept |
Adds points from a brace-enclosed initializer list.
| [in] | points | The initializer list of points to add. |
Definition at line 324 of file surfel.hpp.
|
inlineconstexprnoexcept |
Merges another surfel into this one using a numerically stable parallel update rule.
| surfel | The surfel to merge in. |
If this surfel is empty the other surfel's statistics are copied directly. Otherwise the scatter matrix is updated with the cross-term correction:
where mu and mu_other are the current means. This is the parallel variant of Welford's algorithm.
Definition at line 258 of file surfel.hpp.
|
inlineconstexprnoexcept |
Adds a single 3D point using an online (Welford-like) update rule.
| point | The point to incorporate. |
If this surfel is empty the point initializes the sum and the scatter matrix remains zero. Otherwise the scatter matrix is updated with:
Definition at line 279 of file surfel.hpp.
|
inlinestaticconstexprprotectednoexcept |
Helper to add the outer product correction term in the parallel update.
| [in,out] | ss | The scatter matrix upper triangle to update. |
| [in] | v | The vector used in the outer product correction. |
| [in] | scale | The scaling factor for the correction term. |
Definition at line 645 of file surfel.hpp.
|
inlinestaticconstexprprotectednoexcept |
Computes scatter statistics (ss, sum, n) for a batch of points.
| InputIt | std::input_iterator whose value type is convertible to Vec3d. |
| [in] | first | The beginning of the input range. |
| [in] | last | The end of the input range. |
ss: An array of six doubles representing the upper triangle of the scatter matrix.sum: The sum of all point positions as a Vec<3, double>.n: The number of points in the batch as a std::uint32_t.Accumulates raw second moments in a single pass, then subtracts the centering correction s[i]*s[j]/n to produce the scatter matrix.
Definition at line 676 of file surfel.hpp.
|
inlineconstexprnoexcept |
Clears the surfel to an empty state (no points, zero statistics).
After calling this method, empty() will return true and all statistics will be reset to zero.
Definition at line 473 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns the 3×3 sample covariance matrix.
Computed as sum_squares_ / (n − 1) where n = numPoints(). The matrix is symmetric; both upper and lower triangles are filled.
numPoints() >= 2. Definition at line 511 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns true if no points have been added.
Definition at line 187 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns the centroid (mean position) of all accumulated points.
!empty() - behavior is undefined if the surfel contains no points. Definition at line 491 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns the surface normal as the eigenvector of the covariance matrix corresponding to the smallest eigenvalue.
numPoints() >= 2. Definition at line 532 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns the number of points currently represented by this surfel.
Definition at line 569 of file surfel.hpp.
Merges another surfel into this one using a numerically stable parallel update rule.
| [in] | rhs | The surfel to merge in. |
*this after merging. Definition at line 200 of file surfel.hpp.
Adds a single 3D point to this surfel using an online (Welford-like) update rule.
| [in] | rhs | The point to add. |
*this after adding the point. Definition at line 213 of file surfel.hpp.
Removes another surfel's contribution from this one using a numerically stable parallel update rule.
| [in] | rhs | The surfel to remove. |
*this after removal. Definition at line 340 of file surfel.hpp.
Removes a single 3D point from this surfel and returns *this.
| [in] | rhs | The point to remove. |
*this after removal. Definition at line 352 of file surfel.hpp.
|
constexprdefaultnoexcept |
Equality comparison on all three stored statistics.
|
inlineconstexprnoexcept |
Returns a planarity measure in [0, 1].
Computed as 2 * (e1 - e0) / (e0 + e1 + e2) where e0 <= e1 <= e2 are the sorted eigenvalues of the covariance matrix. A value of 1 indicates a perfectly planar arrangement; 0 indicates an isotropic distribution.
numPoints() >= 2. Definition at line 554 of file surfel.hpp.
|
inlineconstexpr |
Removes all points in [first, last) using a batch algorithm.
| InputIt | std::input_iterator whose value type is convertible to Vec3d. |
| [in] | first | The beginning of the input range. |
| [in] | last | The end of the input range. |
Computes the batch's scatter statistics then reverses the parallel merge.
Definition at line 430 of file surfel.hpp.
|
inlineconstexpr |
Removes all points in points using the batch algorithm.
| Points | std::ranges::input_range whose elements are convertible to Vec3d. |
| [in] | points | The input range of points. |
Definition at line 447 of file surfel.hpp.
|
inlineconstexprprotected |
Helper to remove a batch of points represented by their scatter statistics.
| [in] | ss | The scatter matrix upper triangle of the batch to remove. |
| [in] | sum | The sum of all point positions in the batch. |
| [in] | num_points | The number of points in the batch. |
Definition at line 746 of file surfel.hpp.
|
inlineconstexprnoexcept |
Removes all points in a brace-enclosed initializer list.
| [in] | points | The input range of points. |
Definition at line 457 of file surfel.hpp.
|
inlineconstexprnoexcept |
Removes a previously-added surfel's contribution from this one.
| [in] | surfel | The surfel to subtract. |
If surfel contains at least as many points as this surfel, the surfel is cleared entirely. Otherwise the scatter matrix is updated by reversing the parallel merge formula:
Definition at line 396 of file surfel.hpp.
|
inlineconstexprnoexcept |
Removes a single previously-added 3D point.
| [in] | point | The point to remove. |
Clears the surfel if it is empty or contains exactly one point. Otherwise reverses the online update rule used in add(Vec3f):
Definition at line 413 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns the raw sum of all point positions.
This is the unnormalized sum of all point positions. The mean can be obtained by dividing this sum by numPoints(), or by calling the mean() method directly.
Definition at line 584 of file surfel.hpp.
|
inlineconstexprnoexcept |
Returns the raw scatter matrix upper triangle (Sxx, Sxy, Sxz, Syy, Syz, Szz).
Dividing by (numPoints() − 1) yields the sample covariance upper triangle.
Definition at line 599 of file surfel.hpp.
|
inlinestaticconstexprprotectednoexcept |
Converts an array of six floats to doubles for intermediate calculations.
| [in] | arr | The input array of six floats representing the upper triangle of the scatter matrix. |
doubles converted from the input floats. Definition at line 616 of file surfel.hpp.
|
inlinestaticconstexprprotectednoexcept |
Converts an array of six doubles to floats for storage.
| [in] | src | The input array of six doubles. |
| [out] | dst | The output array of six floats. |
Definition at line 631 of file surfel.hpp.
Returns a new surfel that is the merge of lhs and rhs.
| [in] | lhs | The left-hand side surfel. |
| [in] | rhs | The right-hand side surfel. |
Definition at line 226 of file surfel.hpp.
Returns a new surfel that is the result of adding point rhs to lhs.
| [in] | lhs | The left-hand side surfel. |
| [in] | rhs | The point to add. |
Definition at line 238 of file surfel.hpp.
Returns a new surfel that is the result of removing rhs from lhs.
| [in] | lhs | The left-hand side surfel. |
| [in] | rhs | The surfel to remove. |
rhs removed from lhs. Definition at line 365 of file surfel.hpp.
Returns a new surfel that is the result of removing point rhs from lhs.
| [in] | lhs | The left-hand side surfel. |
| [in] | rhs | The point to remove. |
rhs removed from lhs. Definition at line 377 of file surfel.hpp.
|
protected |
The number of contributing points. This is used to compute the mean and covariance from the raw sums. If this is zero, the surfel is considered empty and all statistics are zero. If this is one, the mean is equal to the single point and the covariance is undefined (but treated as zero). For two or more points, the mean and covariance are computed from the sums as described in the method documentation.
Definition at line 792 of file surfel.hpp.
|
protected |
The sum of all point positions (a Vec<3, float>). This is the unnormalized sum; the mean can be obtained by dividing this sum by numPoints(), or by calling the mean() method directly.
Definition at line 783 of file surfel.hpp.
|
protected |
Upper triangle of the scatter matrix S, stored as six floats in the order (Sxx, Sxy, Sxz, Syy, Syz, Szz). S accumulates the sum of squared deviations from the running mean; the sample covariance is S / (n − 1).
Definition at line 776 of file surfel.hpp.