Using libpointmatcher for 2D lidars extrinsic calibration

[artemiialessandrini]

I'm actually using ROS matcher_service from ethzasl_icp_mapper package, get PointClouds matched and receive a transformation. Here's my client code: https://gist.github.com/artemiialessandrini/e06a6c0d7f7d0bea93169f3a9fe197de My icp.yaml looks like this:

referenceDataPointsFilters:
 - MaxDistDataPointsFilter:
    maxDist: 5.0
 - RandomSamplingDataPointsFilter:
    prob: 0.5

readingDataPointsFilters:
  - RandomSamplingDataPointsFilter:
     prob: 0.8

matcher:
  KDTreeMatcher:
    knn: 3
    epsilon: 0
    maxDist: 8.25

outlierFilters:
  - TrimmedDistOutlierFilter:
     ratio: 0.85

errorMinimizer:
  IdentityErrorMinimizer

transformationCheckers:
  - DifferentialTransformationChecker:
      minDiffRotErr: 0.001
      minDiffTransErr: 0.01
      smoothLength: 4
  - CounterTransformationChecker:
      maxIterationCount: 40
  - BoundTransformationChecker:
      maxRotationNorm: 0.80
      maxTranslationNorm: 8.00

inspector:
 NullInspector

logger:
  FileLogger

But I'm only getting 0 0 0 0 0 0 1. Could someone reproduce an issue or give any tips for 2D? I cannot use force2D: 1, because it's already 2D:)

[simonpierredeschenes]

I am not sure to understand what the problem is, do you get an output translation of 0 ([0, 0, 0]^T)? If you add a VTKFileInspector inspector to your yaml configuration file, libpointmatcher will save the reading point cloud at each iteration. Then, you will be able to see what is happening when the clouds are matched. The inspector configuration looks like this :

VTKFileInspector:
    baseFileName: SOME_OUTPUT_PATH
    dumpReading: 1
    dumpReference: 1
    writeBinary: 1

If you cannot find the issue with the inspector, it would help if you could provide an example cloud to help us reproduce the issue on our side.

[artemiialessandrini]

Thank you, @simonpierredeschenes , for quick response, I'll toggle an inspector on to visualize.

I expect matcher_service from ethzasl_icp_mapper to provide a geometry_msgs/Transform between two coordinate frames of PointClouds. Here's my bag file, to reproduce the issue, please run:

rosbag play non_linear.bag rosrun ethzasl_icp_mapper matcher_service _config:=<path>/<to>/icp.yaml rosrun ethzasl_icp_mapper wrapper

Wrapper client could be found here, That's where the matcher_service is, .yaml file configuration is attached on the top.