Constrain transform increment directionally

[raabuchanan]

Hello, I'm just getting started with libpointmatcher so I apologize if this is already documented somewhere but I could not find a way to do what I want.

I have a set of scans where the orientation is very accurate but the translation has significantly more drift. I would like a way to constrain the ICP to be mostly or only translational. My current problem is that around corners if there is not much overlap, libpointmatcher will perform very large rotations to fit the reading pointcloud. But I know this is wrong and want to limit the rotations. I have attached a figure to illustrate the problem.

Is there any way I can achieve this in the parameters of libpointmatcher? Thank you

[raabuchanan]

Here is my current yaml configuration

readingDataPointsFilters:
  - VoxelGridDataPointsFilter:
      vSizeX: 0.005
      vSizeY: 0.005
      vSizeZ: 0.005
      useCentroid: 1

referenceDataPointsFilters:
  - VoxelGridDataPointsFilter:
      vSizeX: 0.005
      vSizeY: 0.005
      vSizeZ: 0.005
      useCentroid: 1

matcher:
  KDTreeMatcher:
    knn: 3
    maxDist: 0.1

outlierFilters:
  - TrimmedDistOutlierFilter:
      ratio: 0.70

errorMinimizer:
  PointToPointErrorMinimizer

transformationCheckers:
  - CounterTransformationChecker:
      maxIterationCount: 60

inspector:
  VTKFileInspector:
     baseFileName : vissteps
     dumpDataLinks : 1
     dumpReading : 1
     dumpReference : 1

logger:
  FileLogger

[pomerlef]

There is currently no option to limit the degrees of freedom to be minimized beyond 2D (x, y, yaw) and 4D (x, y, z, yaw).

Looking at your yaml file and your example images, you might want to explore these points:

• What is the expected distance between your reference and reading? matcher, maxDist: 0.1 might be too small
• TrimmedDistOutlierFilter, ratio: 0.70 means that you expect 70 % inlier, which I doubt. You can explore this parameter too
• make sure that you have a reasonable number of points in the overlapping area (hard to tell with the images)

[raabuchanan]

Thank you, how can I limit the minimization to 4D (x, y, z, yaw)?

[pomerlef]

PointToPlaneErrorMinimizer
Point-to-plane error (or point-to-line in 2D). Per [9].
- force2D (default: 0) - If set to true(1), the minimization will be forced to give a solution in 2D (i.e., on the XY-plane) even with 3D inputs. - min: 0 - max: 1
- force4DOF (default: 0) - If set to true(1), the minimization will optimize only yaw and translation, pitch and roll will follow the prior. - min: 0 - max: 1