EstimateRigidTransformation in pcl::registration::TransformationEstimation3Point may give non-rigid rotations?

albamath commented 8 years ago

Expected Behavior

I would expect EstimateRigidTransformation to always give invertible affine transformations. In the case of TransformationEstimation3Point I would expect the linear part of the computed transformation to be always a rotation.

For instance, if the source cloud is composed of points (0,0,0), (1,0,0) and (-1,0,0) and target cloud is composed of (0,0,0), (0,1,0) and (0,-1,0) then I would expect that estimateRigidTransformation will give me back anything of the form

0 -sin(θ) cos(θ) 0
1      0      0  0
0  cos(θ) sin(θ) 0
0      0      0  1

for example

or

Current Behavior

This function actually gives invertible affine transformation whose linear part is a rotation when you give her two clouds in which the points define an affine plane. But when you feed it with size 3 point clouds that are co-linear the results might be unexpected. Every time this happens, the computed transformation's linear part will not be of full rank (it will be either rank one or null).

For instance, if the source cloud is composed of points (0,0,0), (1,0,0) and (-1,0,0) and target cloud is composed of (0,0,0), (0,1,0) and (0,-1,0) then the result will be the following transformation

Possible Solution

I think that there should be a test to check whether one of the point clouds has all three points identical, and that in that case, the computed transform should be a translation (the linear part is identity).

I think that there should also be a test to check whether one of the point clouds has all three points co-linear, but I am not sure what the preferred answer in this case should be. If there is no unique preferred solution in this case, maybe the user should be warned.

Code to Reproduce

    pcl::registration::TransformationEstimation3Point<PointXYZ, PointXYZ> te;
    pcl::registration::TransformationEstimation3Point<PointXYZ, PointXYZ>::Matrix4 R;
    pcl::PointCloud<PointXYZ> src, tgt;
    src.resize(3); tgt.resize(3);
    src.points[0].getVector3fMap() << 0.0, 0.0, 0.0;
    src.points[1].getVector3fMap() << 1.0, 0.0, 0.0;
    src.points[2].getVector3fMap() << -1.0, 0.0, 0.0;
    std::cout << "source cloud is " << endl << src.getMatrixXfMap(3, 4, 0) << endl;
    tgt.points[0].getVector3fMap() << 0.0, 0.0, 0.0;
    tgt.points[1].getVector3fMap() << 0.0, 1.0, 0.0;
    tgt.points[2].getVector3fMap() << 0.0, -1.0, 0.0;
    std::cout << "target cloud is " << endl << tgt.getMatrixXfMap(3, 4, 0) << endl;
    te.estimateRigidTransformation(src, tgt, R);
    std::cout << "computed transformation is \n" << R << endl;

Context

Given two points P and Q , I needed a rotation that sends the first one as close as possible to the second one while fixing the origin O and such that the axis of the rotation is orthogonal both to OP and OQ. I did it using pcl::registration::TransformationEstimation3Point::estimateRigidTransform but I had to be very careful on its arguments because of this issue.

P.S. I am still wondering whether this issue is a bug or a feature.

andersgb1 commented 8 years ago

In your code example you have not allocated the src and tgt containers?

albamath commented 8 years ago

You are right! I edited it and added a line with src.resize(3) and tgt.resize(3), and also added two lines to display source and target clouds as column vectors.

Sorry, I was not on a development machine when reporting the issue and thus could not run this code example for real at that moment. I deduced what the computed transformation would be by following the code carefully. Visibly I was wrong, since now that I ran it, on a Windows machine I get :

-1.#IND -1.#IND -1.#IND -1.#IND
-1.#IND -1.#IND -1.#IND -1.#IND
-1.#IND -1.#IND -1.#IND -1.#IND
      0       0       0       1

(Clearly, this output is not correct neither.)

I tried replacing in my code example the line src.points[0].getVector3fMap() << 0.0, 0.0, 0.0; with src.points[0] = PointXYZ(0.0f, 0.0f, 0.0f); etc. but I still get a matrix full of -INFINITYs as an answer.

SergioRAgostinho commented 8 years ago

But when you feed it with size 3 point clouds that are co-linear the results might be unexpected.

They're not only colinear but also equally spaced, meaning that there's more than one solution to the rigid transformation.

I think that there should be a test to check whether one of the point clouds has all three points identical, and that in that case, the computed transform should be a translation (the linear part is identity).

The test can only be done on the rank of the result and there should be a warning if the resultant matrix hasn't got full rank, definitely. But the second part does not make sense. If you have degenerate data, then the user needs to know and that's it. Whatever the method outputs will always be trash.

The "take home" message for me is that the warning needs to be there, other than not really a bug.

stale[bot] commented 4 years ago

Marking this as stale due to 30 days of inactivity. It will be closed in 7 days if no further activity occurs.

PointCloudLibrary / pcl