Singular Hessian for PointToPoint Covariance Estimate

[mnissov]

I've tried now with a couple different pointclouds, and the PointToPointWithCovErrorMinimizer seems to reliably calculate singular Hessian matrices. This is problematic because for the covariance estimate the hessian needs to be inverted.

This is the case when applying ICP between libpointmatcher/examples/data/cloud.00000.vtk and libpointmatcher/examples/data/cloud.00001.vtk, between libpointmatcher/examples/data/cloud.00000.vtk and libpointmatcher/examples/data/cloud.00002.vtk, and as well for a monkey.ply I got from blender in the same fashion as the pcl tutorial

See here an example of one Hessian I got from aligning cloud00000.vtk with cloud00002.vtk for 1 iteration:

J_hessian: (6, 6)
      18742       18742       18742  -0.0192056 -0.00619483  -0.0943748
      18742       18742       18742  -0.0192056 -0.00619483  -0.0943748
      18742       18742       18742  -0.0192056 -0.00619483  -0.0943748
 -0.0192056  -0.0192056  -0.0192056      596964     -182314     -414650
-0.00619483 -0.00619483 -0.00619483     -182314      533133     -350816
 -0.0943748  -0.0943748  -0.0943748     -414650     -350816      765466

I've attached my parameter file, but it is relatively basic. I just went for a super simple ICP chain which would successfully find the transformation I applied to monkey.ply:

// Defining a rotation matrix and translation vector
  Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
  // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
  double theta = M_PI / 8;  // The angle of rotation in radians
  transformation_matrix(0, 0) = std::cos(theta);
  transformation_matrix(0, 1) = -sin(theta);
  transformation_matrix(1, 0) = sin(theta);
  transformation_matrix(1, 1) = std::cos(theta);
  // translation on x axis (0.5 meters)
  transformation_matrix(0, 3) = 0.5;
  // translation on y axis (0.15 meters)
  transformation_matrix(1, 3) = 0.15;

Parameter File

readingDataPointsFilters:
  - RemoveNaNDataPointsFilter
  # - RandomSamplingDataPointsFilter:
  #     prob: 0.5

referenceDataPointsFilters:
  - RemoveNaNDataPointsFilter
#   - SamplingSurfaceNormalDataPointsFilter:
#       knn: 10

matcher:
  KDTreeMatcher:
    knn: 1
    epsilon: 0 

outlierFilters:
  - TrimmedDistOutlierFilter:
      ratio: 0.75

errorMinimizer:
  PointToPointWithCovErrorMinimizer:
    sensorStdDev: 1.0

transformationCheckers:
  - CounterTransformationChecker:
      maxIterationCount: 100
  - DifferentialTransformationChecker:
      minDiffRotErr: 1e-9 # 0.001
      minDiffTransErr: 1e-9 # 0.01
      smoothLength: 4   

#inspector:
#  NullInspector

inspector:
  # PerformanceInspector
  VTKFileInspector:
    baseFileName: pointmatcher-run1
    dumpPerfOnExit: 0
    dumpStats: 0
    dumpIterationInfo: 1 
    dumpDataLinks: 0 
    dumpReading: 0 
    dumpReference: 0 


logger:
  NullLogger
#  FileLogger