Orientations (yaw, pitch and roll) are very different for similar 3D points

[preethamam]

Hi David:

I needed your help really bad.

I have two 3D point clouds that are oriented very similarly. When I use the orientedbox3d,

% Zero center the point cloud data
pcMean  = mean(ptCloudWork.Location, 1);
pcZeroCentered = bsxfun(@minus, ptCloudWork.Location, pcMean);

% Find the axis-orientation of the point cloud
OOBB = orientedBox3d(double(pcZeroCentered));

the orientations, yaw. pitch and roll angles are very different. Aren't they should be similar?
Figure 1,
OOBB1 = -2.7616 -1.2016 -3.5781 223.5262 215.7089 54.8219 -178.3561 1.8756 -7.0416
Figure 3,
OOBB3 = 0.9134 -1.2887 -5.8636 216.6893 214.4492 62.4261 -87.0472 8.7291 -178.4361

Below are figures 1 and 3:

The main objective is to re-orient the point clouds parallel to XY, YZ, and XZ planes. Below is the code snippet that does it.

% Transformation matrix
deg = [OOBB(7)  OOBB(8) OOBB(9)];
mat1 = eulerAnglesToRotation3d(deg,axes);
rotMatNew = inv(mat1);
ptCloudNew.Location = transformPoint3d(pcZeroCentered, rotMatNew);

The reorientation yaw, pitch and roll angles are:
Figure 2,
OOBB2 = 2.9106 1.5406 -3.3209 223.5262 215.7089 54.8219 -0.0000 0.0000 0.0000
Figure 4,
OOBB4 = -2.8909 -0.9726 5.6143 283.9288 286.8387 62.4261 -134.6658 -0.0000 0.0000

Output figures after re-orientation are below. They should reorient similar to figures 1 and 3 (by planes). But what I am getting is totally different.

How do get aligned point clouds to figure 1 and 3 that are reoriented parallel to XY, YZ, and XZ planes?

@dlegland, Please help. I need to fix this soon to get a project going.

[oqilipo]

Are you still looking for a solution?

[preethamam]

Yes, I am.

[dlegland]

Hi @oqilipo!
I've been in contact with preethamam by mail;
I have just had a quick look at the problem, but could not dig very deep... If you have time to check it, you're welcome!
I think I can have a closer look on Friday.

[oqilipo]

@preethamam
Can you upload the data or some example data?

[oqilipo]

What does the variable 'axes' contain in your code snippet?

mat1 = eulerAnglesToRotation3d(deg,axes);

I extended the example of orientedBox3d a little bit. Maybe this helps:

clearvars; close all

[v, f] = sphereMesh;
phi=-360+720*rand;
theta=-360+720*rand;
psi=-360+720*rand;
angles = [phi, theta, psi];
rotMat = eulerAnglesToRotation3d(angles);
rotMat(1:3,4) = randi([-100,100],3,1);
pts = transformPoint3d(bsxfun(@times, v, randi([1,10],1,3)), rotMat);
box3d = orientedBox3d(pts);
figure; drawPoint3d(pts, '.'); hold on;
axis equal;
h = drawCuboid(box3d);
set(h, 'facecolor', 'none');


pts2 = transformPoint3d(pts, inv(createTranslation3d(box3d(1:3))*eulerAnglesToRotation3d(box3d(7:9))));
figure; drawPoint3d(pts2, '.'); hold on;
axis equal;

[preethamam]

@oqilipo, axes is 'ZYX'. Here is the data: Data.zip. Please let me know if anything is required.

[oqilipo]

Hi preethamam
Please provide the point cloud as MAT file stored in a simple array.
Thank you

[preethamam]

@oqilipo here is the data: Data.zip

[preethamam]

@oqilipo just in case if you need the full program to produce the exact issue that I have raised. Here I have attached it: Full program and data.zip.

[oqilipo]

Maybe this is what you want to do?

clearvars; close all

load('Data.mat')

% Target points
pts3 = figure3_tgtLocation;
ell3d3 = equivalentEllipsoid(pts3);

% Source points
pts1 = figure1_srcLocation;
ell3d1 = equivalentEllipsoid(pts1);

% Transform source to target
TFM1to3 = ...
    createTranslation3d(ell3d3(1:3))*eulerAnglesToRotation3d(ell3d3(7:9))*...
    inv(createTranslation3d(ell3d1(1:3))*eulerAnglesToRotation3d(ell3d1(7:9)));
pts1to3 = transformPoint3d(pts1, TFM1to3);

figure('color','w');
drawPoint3d(pts3, '.b')
hold on
drawPoint3d(pts1to3, '.g');
axis equal
legend({'target';'source2target'})

[preethamam]

@oqilipo what was wrong with oriented box 3D? Could you please help me with how can it be done with oriented box 3D? Because I feel it is easier to visualize the orientation of the box than an ellipsoid.

[dlegland]

Hi there,

I played a bit with the data, and here are few thoughts:

• the 2D projections of the two point clouds are nearly square. Therefore, it may bevery likely that resulting bounding boxes differ by a 90 degree rotation around an axis close to the vertical. I suspect there is such a difference in the result of "orientedBox3d" for the two point clouds
• using equivalent ellipsoid may sometimes be more "robust", in the sense that it is less dependent on extreme values. In point cloud 2 ('target'), there is a point with very low z-coordinates. This induces a large difference in oriented box, with the effect is negligible with ellipsoid
• However, I agree that the example is easier to visualize using oriented box than ellipsoid...
• I tried the same approach as @oqilipo for computing "TFM1to3" , but using angles of oriented boxes instead. I got an unwanted rotation by 180 degrees, making the final boxes to overlap, but not the points...

It seems to be difficult to find a generic and robust way to register point clouds based on global features alone... Maybe a more global algorihtm such a Iterative Closest Point could by more effective in this case?

Best,
David

[preethamam]

Hello Everybody,

Thank you for the suggestions and answers!

The intention of using the oriented box 3D for source and target point clouds is to move them as much closer into the same coordinate reference. Then, later to use Iterative Closest Point to fine register them. This has two advantages, to help ICP and to create ground-truth orientations that can train deep learning-based models.

I used the code posted by @oqilipo and plotted the ellipsoids. Here is what I got:

Why are some points are outside after fitting an equivalent ellipsoid? Also, it was quite difficult to understand the orientations of the axes. Shouldn't they be almost parallel to the XYZ axes, as the point clouds are already?

When I plot the point clouds, I see they are already close to each other in this example.

Would you please let me know if there are any other robust ways to align the point clouds parallel to XYZ axes?

[oqilipo]

You may use equivalentEllipsoid for the registration of the source to the target (this is a sufficient prealligment for an rICP) and afterwards you use orientedbox3d to create a transformation to align the data with the XYZ planes. However, the latter might not be necessary.

[preethamam]

But, why are some points are outside after fitting an equivalent ellipsoid? How to understand the orientations of the axes. Shouldn't they be almost parallel to the XYZ axes, as the point clouds are already?

[oqilipo]

equivalentEllipsoid uses a PCA. See Wikipedia for more information.

[dlegland]

Hi,
as a follow up, a function "registerPoints3d_icp" has been added to the "geom3d" module. It is based on another contribution from FileExchange by Hans Martin Kjer & Jakob Wilm. It is expected to solve this kind of problems, so this should worth a try!
Caution: I did not implement all the features from the original file. However, the conventions from original file differ with thoses of MatGeom, makjing interoperability more complex.

[preethamam]

Thanks @dlegland! I will look into it.

[oqilipo]

Hi, as a follow up, a function "registerPoints3d_icp" has been added to the "geom3d" module. It is based on another contribution from FileExchange by Hans Martin Kjer & Jakob Wilm. It is expected to solve this kind of problems, so this should worth a try! Caution: I did not implement all the features from the original file. However, the conventions from original file differ with thoses of MatGeom, makjing interoperability more complex.

registerPoints3d_icp.m and registerPoints3dAffine.m could be combined into one function registerPoints3d.m and the user can select the method by in input parameter?

[dlegland]

registerPoints3d_icp.m and registerPoints3dAffine.m could be combined into one function registerPoints3d.m and the user can select the method by in input parameter?

Yes, that could be an option. One has to check if output args are similar in both cases. As both can be represented with Affine Transform matrices that should work. I'll try to have a look.

[oqilipo]

Yes, that could be an option. One has to check if output args are similar in both cases. As both can be represented with Affine Transform matrices that should work. I'll try to have a look.

I can have a look into it.

[dlegland]

I can have a look into it.

Yes, if you have time this can be fine.

[oqilipo]

See #198

[oqilipo]

I've added an example in the header of "registerPoints3d"

It works for the 'icp' algorithm, but not for the 'affine' algorithm. You might have a look at it.