robokudo.annotators.shape_estimator

Shape estimation for object hypotheses.

This module provides a minimal, pure-Python annotator that fits primitive shape models to segmented object point clouds.

Classes

CylinderAxisCandidate	Candidate axis direction together with a refitted cylinder.
ShapeEstimatorAnnotator	Estimate one primitive shape per object hypothesis point cloud.

Module Contents

class robokudo.annotators.shape_estimator.CylinderAxisCandidate

Candidate axis direction together with a refitted cylinder.

source: str

axis_direction: numpy.ndarray

fit: robokudo.utils.shape_fitting.CylinderFit

class robokudo.annotators.shape_estimator.ShapeEstimatorAnnotator(name: str = 'ShapeEstimatorAnnotator', descriptor: ShapeEstimatorAnnotator | None = None)

Bases: robokudo.annotators.core.ThreadedAnnotator

Estimate one primitive shape per object hypothesis point cloud.

Overview: 1. Validate input cloud (minimum_point_count) and optionally run

statistical outlier removal.

2. Fit enabled primitives (sphere/cylinder/cuboid) on the same filtered points.

3. Apply post-fit stabilization: - Cylinder: optional axis stabilization from principal/preferred axes. - Cuboid: optional in-plane orientation stabilization for near-square faces.

4. Rank accepted candidates with select_best_shape(…).

5. Convert the winning fit to a RoboKudo annotation (Sphere, Cylinder, or Cuboid), keep inlier indices mapped to the original object cloud, and publish visualization geometries.

Parameter groups: - Global data quality: minimum_point_count, outlier removal settings. - Shared fit behavior: distance_threshold, robust_loss. - Shape gates:

sphere/cylinder/cuboid enable flags plus per-shape size and ratio limits.

• Candidate selection: selection_score_tolerance and cuboid preference settings for close scores.

• Stabilization controls: cylinder axis and cuboid orientation stabilization.

Practical tuning order: 1. Set hard physical limits (max radii/heights/extents) for your scene. 2. Tune inlier thresholds (distance_threshold, per-shape min inlier ratio). 3. Tune selection preference parameters. 4. Only then tune stabilization triggers/tolerances.

This annotator intentionally favors robust, bounded fits for household-scale objects.

class Descriptor

Bases: robokudo.annotators.core.BaseAnnotator.Descriptor

Configuration descriptor for shape estimation.

class Parameters

Parameter container for shape estimation.

minimum_point_count: int = 80

Minimum number of points required for fitting.

distance_threshold: float = 0.01

Inlier threshold used for all shape residuals.

robust_loss: str = 'soft_l1'

Robust loss used for non-linear least squares.

minimum_inlier_ratio: float = 0.5

Minimum inlier ratio required for publishing a shape.

remove_statistical_outliers: bool = False

Whether statistical outlier removal is applied before fitting.

outlier_neighbor_count: int = 30

Neighbor count for statistical outlier removal.

outlier_standard_deviation_ratio: float = 2.0

Standard deviation ratio for statistical outlier removal.

fit_sphere: bool = False

Whether sphere fitting is enabled.

max_sphere_radius_meters: float = 0.4

Maximum allowed sphere radius.

max_sphere_radius_to_bbox_diagonal_ratio: float = 1.0

Maximum allowed ratio between sphere radius and point cloud diagonal.

max_sphere_radius_to_observed_extent_ratio: float = 0.8

Maximum allowed ratio between sphere radius and observed cloud extent.

max_sphere_center_distance_to_bbox_diagonal_ratio: float = 0.9

Maximum allowed ratio between center offset and point cloud diagonal.

fit_cylinder: bool = True

Whether cylinder fitting is enabled.

cylinder_minimum_inlier_ratio: float = 0.87

Minimum inlier ratio required for cylinder candidates.

max_cylinder_radius_meters: float = 0.4

Maximum allowed cylinder radius.

max_cylinder_height_meters: float = 1.0

Maximum allowed cylinder height.

max_cylinder_radius_to_bbox_diagonal_ratio: float = 1.0

Maximum allowed ratio between cylinder radius and cloud diagonal.

max_cylinder_radius_to_cross_section_extent_ratio: float = 0.95

Maximum allowed ratio between cylinder radius and observed cross-section extent.

max_cylinder_center_distance_to_bbox_diagonal_ratio: float = 0.8

Maximum allowed ratio between axis center offset and cloud diagonal.

cylinder_max_initializations: int = 12

Maximum number of cylinder initialization hypotheses for multi-start optimization.

cylinder_consensus_trials: int = 30

Number of random axis hypotheses used to seed cylinder multi-start optimization.

cylinder_inlier_polishing_iterations: int = 0

Number of post-optimization inlier-only polishing iterations for cylinder fitting.

stabilize_cylinder_axis_with_principal_axis: bool = True

Whether cylinder axis should be stabilized using point principal axis.

cylinder_axis_stabilization_trigger_degrees: float = 8.0

Minimum axis deviation angle required to trigger cylinder stabilization.

cylinder_axis_stabilization_max_score_drop: float = 0.08

Maximum tolerated score drop for accepting stabilized cylinder.

prefer_upright_cylinder_axis: bool = True

Whether an upright preferred axis should be favored when scores are close.

preferred_cylinder_axis_direction: typing_extensions.Tuple[float, float, float] = (0.0, 0.0, 1.0)

Preferred global cylinder axis direction used as stabilization prior.

upright_cylinder_axis_score_tolerance: float = 0.05

Maximum score gap for preferring a more upright cylinder axis.

fit_cuboid: bool = True

Whether cuboid fitting is enabled.

max_cuboid_extent_meters: float = 0.6

Maximum allowed cuboid edge length for any axis.

cuboid_distance_threshold: float = 0.015

Inlier threshold used specifically for cuboid fitting.

cuboid_minimum_inlier_ratio: float = 0.35

Minimum inlier ratio required for cuboid candidates.

selection_score_tolerance: float = 0.05

Maximum score gap for considering two fits equivalent.

prefer_cuboid_when_score_close: bool = True

Whether cuboids should be preferred when scores are close.

cuboid_preference_score_margin: float = 0.06

Maximum score gap to still prefer a box-like cuboid candidate.

cuboid_preference_inlier_ratio_tolerance: float = 0.05

Maximum inlier-ratio gap to still prefer a cuboid.

cuboid_box_like_cross_section_asymmetry_threshold: float = 0.12

Minimum cross-section asymmetry to classify a cuboid as box-like.

cuboid_box_like_cube_axis_similarity_tolerance: float = 0.12

Maximum axis spread ratio to classify a cuboid as cube-like.

stabilize_ambiguous_cuboid_orientation: bool = True

Whether ambiguous cuboid in-plane orientation should be stabilized.

cuboid_ambiguous_in_plane_extent_relative_difference: float = 0.3

Maximum relative in-plane extent difference treated as orientation-ambiguous.

log_candidate_metrics: bool = True

Whether candidate metrics are logged on debug level.

log_rejection_reasons: bool = True

Whether shape rejection reasons are logged when metrics logging is enabled.

parameters

compute() → py_trees.common.Status

Estimate shapes and publish annotations and 3D visualizations.

_estimate_shape_for_object_hypothesis(object_hypothesis: robokudo.types.scene.ObjectHypothesis) → typing_extensions.Optional[typing_extensions.Tuple[robokudo.types.annotation.Shape, typing_extensions.List[typing_extensions.Dict[str, typing_extensions.Any]]]]

Estimate one shape candidate for a single object hypothesis.

_valid_point_cloud(point_cloud: typing_extensions.Optional[open3d.geometry.PointCloud]) → bool

Return whether a point cloud has enough points for shape estimation.

_collect_shape_candidates(object_hypothesis: robokudo.types.scene.ObjectHypothesis, points: numpy.ndarray) → typing_extensions.List[robokudo.utils.shape_fitting.FittedShape]

Fit all enabled primitives and return accepted candidates.

_enabled_shape_fit_requests() → typing_extensions.List[typing_extensions.Tuple[robokudo.utils.shape_estimator_adapters.ShapeFitAdapter, robokudo.utils.shape_estimator_adapters.ShapeFitParameters]]

Return enabled shape fitters with explicit fit parameters.

_record_shape_candidate(candidates: typing_extensions.List[robokudo.utils.shape_fitting.FittedShape], object_hypothesis: robokudo.types.scene.ObjectHypothesis, shape_name: str, fit_result: typing_extensions.Optional[robokudo.utils.shape_fitting.FittedShape]) → None

Append accepted candidate or log its rejection.

_postprocess_shape_candidate(fit_result: typing_extensions.Optional[robokudo.utils.shape_fitting.FittedShape], points: numpy.ndarray) → typing_extensions.Optional[robokudo.utils.shape_fitting.FittedShape]

Apply annotator-level stabilization policies to fitted shape candidates.

_cylinder_fit_constraints() → robokudo.utils.shape_fitting.CylinderFitConstraints

Return cylinder constraints from descriptor parameters.

_select_best_shape_candidate(candidates: typing_extensions.List[robokudo.utils.shape_fitting.FittedShape]) → typing_extensions.Optional[robokudo.utils.shape_fitting.FittedShape]

Select the best shape candidate using descriptor preferences.

_prepare_point_cloud(point_cloud: open3d.geometry.PointCloud) → tuple[open3d.geometry.PointCloud, numpy.ndarray]

Return a filtered point cloud and retained index mapping.

_stabilize_cylinder_axis_if_tilted(cylinder_fit: robokudo.utils.shape_fitting.CylinderFit, points: numpy.ndarray) → robokudo.utils.shape_fitting.CylinderFit

Stabilize cylinder axis using principal axes and optional upright prior.

_refit_cylinder_axis_candidates(cylinder_fit: robokudo.utils.shape_fitting.CylinderFit, points: numpy.ndarray) → typing_extensions.List[CylinderAxisCandidate]

Refit configured cylinder axis candidates and include the original fit.

_align_axis_with_reference(axis_direction: numpy.ndarray, reference_axis_direction: numpy.ndarray) → numpy.ndarray

Return an axis direction flipped to agree with a reference direction.

_should_accept_stabilized_cylinder_axis(original_fit: robokudo.utils.shape_fitting.CylinderFit, selected_candidate: CylinderAxisCandidate) → bool

Return whether a stabilized cylinder candidate should replace the original.

_log_stabilized_cylinder_axis(original_fit: robokudo.utils.shape_fitting.CylinderFit, selected_candidate: CylinderAxisCandidate) → None

Log an accepted cylinder-axis stabilization.

_collect_cylinder_axis_candidates(points: numpy.ndarray) → typing_extensions.List[typing_extensions.Tuple[str, numpy.ndarray]]

Return candidate cylinder axis directions for stabilization.

_contains_similar_axis_direction(existing_axis_candidates: typing_extensions.List[typing_extensions.Tuple[str, numpy.ndarray]], candidate_axis_direction: numpy.ndarray, similarity_threshold_degrees: float = 8.0) → bool

Return whether candidate axis is similar to one axis already in the list.

_preferred_cylinder_axis_direction() → typing_extensions.Optional[numpy.ndarray]

Return configured preferred cylinder axis direction if valid.

_select_cylinder_axis_candidate(candidate_fits: typing_extensions.List[CylinderAxisCandidate]) → CylinderAxisCandidate

Select one cylinder axis candidate by score and optional upright preference.

_point_cloud_principal_axes(points: numpy.ndarray) → typing_extensions.List[numpy.ndarray]

Return principal point-cloud axes sorted by descending variance.

_axis_angle_degrees(first_axis: numpy.ndarray, second_axis: numpy.ndarray) → float

Return unsigned angle in degrees between two axis directions.

_normalized_vector(vector: numpy.ndarray) → numpy.ndarray

Return normalized vector with safe fallback for near-zero norm.

_stabilize_cuboid_orientation_if_ambiguous(cuboid_fit: robokudo.utils.shape_fitting.CuboidFit, points: numpy.ndarray) → robokudo.utils.shape_fitting.CuboidFit

Stabilize in-plane cuboid orientation for near-square visible faces.

_cuboid_orientation_axis_indices(extents: numpy.ndarray) → typing_extensions.Tuple[int, int, int]

Return normal-axis index followed by the two in-plane axis indices.

_cuboid_in_plane_orientation_is_ambiguous(extents: numpy.ndarray, first_in_plane_axis_index: int, second_in_plane_axis_index: int) → bool

Return whether in-plane extents are close enough to stabilize.

_stabilized_cuboid_rotation_matrix(rotation_matrix: numpy.ndarray, smallest_axis_index: int, first_in_plane_axis_index: int, second_in_plane_axis_index: int) → typing_extensions.Optional[numpy.ndarray]

Return a cuboid rotation matrix with canonical in-plane axes.

_first_projected_reference_axis(normal_axis: numpy.ndarray) → typing_extensions.Optional[numpy.ndarray]

Return the first global reference axis that projects onto the cuboid plane.

_canonical_cuboid_plane_axes(projected_reference_axis: numpy.ndarray, normal_axis: numpy.ndarray, original_first_axis: numpy.ndarray, original_second_axis: numpy.ndarray) → typing_extensions.Tuple[numpy.ndarray, numpy.ndarray]

Return canonical in-plane axes aligned with the original axis signs.

_refit_cuboid_or_keep_original(cuboid_fit: robokudo.utils.shape_fitting.CuboidFit, points: numpy.ndarray, stabilized_rotation_matrix: numpy.ndarray) → robokudo.utils.shape_fitting.CuboidFit

Refit a stabilized cuboid and keep the original fit on failure.

_project_axis_onto_plane(axis: numpy.ndarray, plane_normal: numpy.ndarray) → numpy.ndarray

Project axis onto the plane orthogonal to the given plane normal.

_map_to_object_indices(object_hypothesis: robokudo.types.scene.ObjectHypothesis, local_indices: numpy.ndarray) → typing_extensions.List[int]

Map local point indices back to object hypothesis index space.

_create_visualization_geometries(object_hypothesis: robokudo.types.scene.ObjectHypothesis, filtered_cloud: open3d.geometry.PointCloud, best_fit: robokudo.utils.shape_fitting.FittedShape) → typing_extensions.List[typing_extensions.Dict[str, typing_extensions.Any]]

Create visualization geometries for one fitted object.

_create_inlier_colored_cloud(cloud: open3d.geometry.PointCloud, inlier_indices: numpy.ndarray) → open3d.geometry.PointCloud

Return a copy of the cloud with inliers and outliers color coded.

_log_rejected_candidate(object_hypothesis: robokudo.types.scene.ObjectHypothesis, shape_name: str) → None

Log one rejected shape candidate.

_log_candidate_metrics(object_hypothesis: robokudo.types.scene.ObjectHypothesis, fit_result: robokudo.utils.shape_fitting.FittedShape) → None

Log shape candidate metrics for debugging.

_log_selected_candidate(object_hypothesis: robokudo.types.scene.ObjectHypothesis, fit_result: robokudo.utils.shape_fitting.FittedShape) → None

Log selected candidate metrics for debugging.