Getting stable “yaw” for robotics: PCA, tabletop, and the beginning of my tracking pipeline.
I am creating a perception pipeline from RGB-D to do pick-and-place with a depth camera.
I need to detect an object on a table, calculate its 3D position, and obtain its orientation: an orientation that allows the robot to grasp the object.
In this text, I will describe some things learned during the first part of the implementation.
Getting useful geometry from RGB-D
The camera, an Intel RealSense D405, gives me two images:
color[y, x]
depth[y, x]The first thing is to align them so that each pixel corresponds to its counterpart.
This is important because the segmentation method I use is 2D: a model I already trained, YOLO26-Segment.
With segmentation and the aligned images, I can select the pixels of the object I want and obtain their depth z.
At that moment, I need to deproject, that is, increase the dimension of the u, v coordinates from the mask, to obtain a 2.5D point cloud of the object with respect to the camera frame:
pixel + depth + intrinsics -> 3D pointIn parallel, I also build the pointcloud of the full scene, useful later in order to compute the table normal.
Defining the table plane with RANSAC
To work in tabletop, I need to know where the table is.
A plane in space can be expressed as:
ax + by + cz + d = 0Where the normal is equivalent to:
[a, b, c]The normal, by definition, is perpendicular to the table plane.
RANSAC gives me that “normal”.
That normal now corresponds to the local Z axis of the table:
Z_table
Building the rest of the local table frame
Having Z_local, I can build the two remaining axes:
base_u = local X axis on the table
base_v = local Y axis on the table
Z_table = axis perpendicular to the tableHaving this frame, I can express any 3D point with respect to it:
[u, v, h]Where:
u = coordinate on base_u
v = coordinate on base_v
h = height with respect to the tableThis was key to understand it: I am changing the way I look at the scene.
Get Daniel Dorado’s stories in your inbox
Join Medium for free to get updates from this writer.
I move from camera coordinates to local table coordinates.
Calculating yaw: Looking at the object from above
To obtain the yaw on the table, I only need:
[u, v]The coordinate h can be left out.
For that, I can use PCA — Principal Component Analysis — to detect, from those [u, v] points in 2D, in which direction they extend the most.
PCA gives me, in this case, the principal axes and the variance of each axis:
principal axis = most elongated direction of the object
secondary axis = short direction, perpendicular to the previous oneI can represent the principal axis as a vector:
major_axis_2d = [a, b]This vector is expressed with respect to the frame and the plane of the table:
a = component on base_u
b = component on base_vThat is, I have the rotation of the object with respect to the local table frame.
The 180-degree ambiguity
When I build a detection box for the object…
If I take a rectangle, look at it from “above” without h, that is, without depth, only in 2D, and also rotate it 180 degrees, I obtain a rectangle with the same visible shape.
This is a problem for the robot: I need stable detection boxes.
It happens because PCA can give me a mathematically correct representation, but in opposite directions.
For PCA, this is correct:
0º == 180ºFor the robot, it is not.
The object has not really rotated. PCA is simply giving me a non-useful representation that will make the rest of the pipeline believe that it has rotated.
Circular mean with 180-degree ambiguity
To solve it, I use this function:
def circular_mean_deg(yaws_deg: list[float]) -> float:
if not yaws_deg:
return 0.0
doubled = np.radians(np.asarray(yaws_deg, dtype=np.float64) * 2.0)
mean = np.arctan2(np.sin(doubled).mean(), np.cos(doubled).mean())
return float(np.degrees(mean) / 2.0)Since PCA returns an axis without direction, θ and θ + 180º represent the same orientation.
To calculate a stable mean, I double the angles before doing the circular mean.
This way, orientations separated by 180 degrees become separated by 360 degrees, which circularly become the same point again.
After that, I divide the result by two to return to the original range.
What does this bring to 3D object tracking?
Stability.
A detection without stability is not very useful and can end up generating detection errors and strange movements on the hardware.
For example:
track.yaws = [42, 43, 222, 41]
track.yaw = circular_mean_deg(track.yaws)In an axial orientation, 222º is equivalent to 42º, because they are separated by 180 degrees.
This way, I stop treating 222º as a real rotation of the object and I can keep averaging by window to obtain much more stable inclinations with respect to the table.
This allows me to build more consistent detections to use later in the perception and manipulation pipeline…
