How to get Rotational Matrix for Oriented Bounding Box in Point Cloud Library

Question

I am using the Axis Aligned bounding box for detected objects from YOLO. Also have the point clouds from my ZED depth Camera. So I would like to find the orientation of this Bounding box using PCL. So how to find the rotation matrix so can rotate the axis aligned box according to the cloud as i need the yaw information from the box.

I know from the PCL Tutorial Using pcl::MomentOfInertiaEstimation

pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();

pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;

feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
feature_extractor.getMassCenter (mass_center);

Then how to get the rotational_matrix_OBBcause for the final OBB coordinates:

Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);

p1 = rotational_matrix_OBB * p1 + position;
p2 = rotational_matrix_OBB * p2 + position;
p3 = rotational_matrix_OBB * p3 + position;
p4 = rotational_matrix_OBB * p4 + position;
p5 = rotational_matrix_OBB * p5 + position;
p6 = rotational_matrix_OBB * p6 + position;
p7 = rotational_matrix_OBB * p7 + position;
p8 = rotational_matrix_OBB * p8 + position;

I need the rotational_matrix_OBB, but how to get it? Thanks

I don't understand the question. Is there any problem with this line: feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); ? — Mark Loyman
Actually with these 3 lines Eigen::Matrix3f rotational_matrix_OBB; Eigen::Vector3f major_vector, middle_vector, minor_vector; Eigen::Vector3f mass_center; How to obtain major_vector, middle_vector, minor_vector,rotational_matrix_OBB and mass_center? Understand now? — Bob9710
ok I got it. So in my case I have point clouds and have 8 points from xmin, xmax, ymin, ymax, zmin and zmax from the YOLO bounding box. So I will use this points to set ROI in the x-axis, y-axis and z-axis. After Set the ROI with PassThrough filtering with 'pass.filter(*cloud);'. Is that correct? Then I like to process only that filter cloud? Is that possible and how to do that? — Bob9710
I only want to proceed and get the OBB only for the cloud which contains data bounded by the YOLO bounding box with the 8 min, max points. But looks like the PassThrough filter is not a good solution. Is it 'pcl::ConditionalRemoval' or 'pcl::CropBox' better solution? — Bob9710

Ugzer Ugzer · Accepted Answer · 2020-07-21T19:06:05

A way to go is to consider the following process:

If the variable X describes your 3D points stacked as lines in an array (shape (N, 3)), let M = X.T * X with X.T the transposed of X and * being the matrices product. M is then a 3x3 symmetric matrix.

Extracting the eigen vectors of this matrix (which exist and are orthogonal according to the spectral theorem), you can then concatenate these vectors taken as column vectors (shape (3, 1)) to obtain as 3x3 orthogonal matrix R, that you can multiply by its determinant to obtain the rotation matrix that represents the orientation of your points.

An interesting fact is that the associated eigen values actually are a way to measure the spreading of your points cloud along the 3 extracted directions. Hence, the eigen vector associated with the biggest eigen value (they are all positive due to the way M is defined) is the one describing the orientation of the object's main axis in space.

The most natural way to order your eigen vectors inside the rotation matrix is to sort them by descending order of their associated eigen value (from left to right). This way, the longest object axis is mapped on the x axis, the second longest on the y axis and the smallest on the z axis.

How to get Rotational Matrix for Oriented Bounding Box in Point Cloud Library

1 Answers