RigidTransformation: Error, rotation matrix is not orthogonal.

[jahead]

Hey so I keep getting this error randomly showing up, in my icp runs.
"RigidTransformation: Error, rotation matrix is not orthogonal."
I don't really understand why it would be throwing this error, or how to solve it.

For example, here is two point clouds that have caused this error.

a gist of it can be found here: https://gist.github.com/jahead/cb5b3add89fc1815452a
and my icp setup is this.

    icp.transformations.clear();
    icp.readingDataPointsFilters.clear();
    icp.readingStepDataPointsFilters.clear();
    icp.referenceDataPointsFilters.clear();
    icp.matcher.reset();
    icp.outlierFilters.clear();
    icp.errorMinimizer.reset();
    icp.transformationCheckers.clear();
    icp.inspector.reset();

    icp.transformations.push_back(new typename TransformationsImpl<float>::RigidTransformation());    

icp.readingDataPointsFilters.push_back(new typename DataPointsFiltersImpl<float>::RemoveNaNDataPointsFilter());
    icp.readingDataPointsFilters.push_back(pointMatcher::get().DataPointsFilterRegistrar.create(
                                                                                                "MaxPointCountDataPointsFilter",
                                                                                                PointMatcherSupport::map_list_of
                                                                                                ("prob",PointMatcherSupport::toParam(1))
                                                                                                ("maxCount", PointMatcherSupport::toParam(100))
                                                                                                )
                                           );
    icp.referenceDataPointsFilters.push_back(new typename DataPointsFiltersImpl<float>::RemoveNaNDataPointsFilter());
    icp.referenceDataPointsFilters.push_back(pointMatcher::get().DataPointsFilterRegistrar.create(
                                                                                                  "MaxPointCountDataPointsFilter",
                                                                                                  PointMatcherSupport::map_list_of
                                                                                                  ("prob",PointMatcherSupport::toParam(1))
                                                                                                  ("maxCount", PointMatcherSupport::toParam(100))
                                                                                                  )
                                             );

    icp.outlierFilters.push_back(pointMatcher::get().OutlierFilterRegistrar.create("MedianDistOutlierFilter",
                                                                                   PointMatcherSupport::map_list_of
                                                                                   ("factor",PointMatcherSupport::toParam(1.1))
                                                                                   ));
    icp.matcher.reset(new typename MatchersImpl<float>::KDTreeMatcher());
    icp.errorMinimizer.reset(new typename ErrorMinimizersImpl<float>::PointToPointErrorMinimizer());
    icp.transformationCheckers.push_back(pointMatcher::get().TransformationCheckerRegistrar.create(
                                                                                                   "CounterTransformationChecker",
                                                                                                   PointMatcherSupport::map_list_of
                                                                                                   ("maxIterationCount", PointMatcherSupport::toParam(100))
                                                                                                   )
                                         );

    icp.transformationCheckers.push_back(new typename TransformationCheckersImpl<float>::DifferentialTransformationChecker());

I also realise that there is a correctParameters function. But your ICP isn't using it once it detects the transformation error being thrown; it's just throwing an exception. It seems odd that the ICP can't do this correction on the fly. By just calling the correctParameters function when checkParameters returns false. Or maybe I'm looking at the problem wrong.

Oh and ps.
I don't load from yaml. Could this be a problem?
And I'm running the icp at high frequency as i'm trying converge two datasets from two different leap motions.

Oh and btw. I'm loving your library

[pomerlef]

Hi Jahead,

Thanks for reporting. I'll need to investigate your problem more precisely. In your example, there are few points and you use an outlier filter which will reduce even more the number of points. Maybe this exception is throw before/instead of a proper one which should say "not enough points" or something like that. I'm on the road for some days so I'll check that later. If you could check that, it would be great.

Meanwhile, here are the answers to your questions:

• The use of yaml or not shouldn't influence the exception. Only the type of errorMinimizer from which this exception is thrown.
• the function correctParameters() is not use automatically because there is often another problem in the code for which it is important to notify the developers. The problem with this function is that it will always find an orthogonal even with a random matrix. If you know that the exception is thrown by rounding error that cumulate or you're very close to an orthogonal matrix, then you can use it. Otherwise, one should be careful with it.

If you develop new modules to suit your needs, we are always happy to have more contributors!

[jahead]

Sorry for the late reply. Thank you for your response.

It does seems the low point count is a problem. The new beta version of the leap motion sdk, is now giving us more points so I've been updating and porting the code to handle a larger points set.

I"ll try not using an outlier filter when I get around to use ICP again; which should be soon. But I wasn't getting the "not enough points" exception. we concluded (not tested) that the icp was not having enough pairs to do its work and the only pairs that were detected were becoming co linear. Our the data points are only approximations that get approximated based separately, therefore the points be can related to the same 'joint' but have slightly different xyz points.

Hope that gives you a bit more context.

[jahead]

The larger the point cloud did help. 22 points and we aren't seeing this exception. Which is interesting that I wasn't getting the "not enough points" exception.

[pomerlef]

Hi jahead,

So looking at the code a bit more. There is no safety for a minimal number of in the errorMinimizer. There was a check for singularity in point-to-plane based on the rank of the matrix but it was commented out because it was throwing too often.

It is tricky to put a hard threshold on the number of points given that in theory it should be 3, but in practice it is often more than that.

I'll reopen another issue with a clearer description in case somebody has already though about it.