Inconsistent transformation results when applying matrix from filters.icp

[jomey]

I am getting different results, when applying the matrix reported back by the filters.icp and using the transform with filters.transformation for the same point could.

Steps I used:

1. Run below pipeline with the --metadata options as described in the docs.

[
   "fixed.laz",
   "moving.laz",
    {
        "type": "filters.icp"
    },
    "aligned_icp.laz"
]

2. Take the transform information from the metadata output and put it into a new pipeline to apply the same transformation to the initial moving cloud.

[
    "moving.laz",
    {
        "type": "filters.transformation",
        "matrix": "           1 -7.96375e-06 -2.36573e-06  -0.00623153\n  7.9637e-06            1 -2.50978e-05     0.048159\n 2.36593e-06  2.50977e-05            1    -0.418688\n           0            0            0            1"
    },
    {
        "type": "writers.las",
        "compression": "laszip",
        "forward": "all",
        "filename": "aligned_transformation.laz"
    }
]

3. I checked the aligned_icp.laz summary versus the alinged_transformation.laz summary and they are not the same.

• Summary: moving.laz

...
  "summary":
  {
    "bounds":
    {
      "maxx": 336552,
      "maxy": 4322445,
      "maxz": 4107.3,
      "minx": 322518,
      "miny": 4305588,
      "minz": 2657.95
    },
...

• Summary: alingned_icp.laz

...
  "summary":
  {
    "bounds":
    {
      "maxx": 336552.064,
      "maxy": 4322445.055,
      "maxz": 4106.927776,
      "minx": 322517.9357,
      "miny": 4305587.991,
      "minz": 2657.314217
    },
...

• Summary: aligned_transformation.laz

...
  "summary":
  {
    "bounds":
    {
      "maxx": 336517.6937,
      "maxy": 4322447.596,
      "maxz": 4216.000738,
      "minx": 322483.5654,
      "miny": 4305590.533,
      "minz": 2766.387178
    },
...

Notes
When I only apply the translation and not the rotation, the results are closer. Applying both, translation and rotation, produces the results that are a lot different.

Expected behavior
Identical transformation for the moving cloud using the output from filters.icp and applying it with filters.transformation

System/installation information:

$ uname -a
Linux 3.10.0-957.27.2.el7.x86_64 #1 SMP Mon Jul 29 17:46:05 UTC 2019 x86_64 x86_64 x86_64 GNU/Linux

pdal 2.0.1 (git-version: 6600e6)

Installed via conda:

pdal                      2.0.1            py37hb614e5c_9    conda-forge

[jomey]

Adding the observation made on the mailing list, plus highlighting the lines of code mentioned:

• IterativeClosestPoint.cpp
• TransformationFilter.cpp

Date: Thu, 5 Mar 2020 17:36:53 -0600
From: Jim Klassen <klassen.js@gmail.com>
To: pdal@lists.osgeo.org
Subject: Re: [pdal] filters.icp transformation
Message-ID: <724eca78-37cb-3e8f-3a46-81ada3d2771b@gmail.com>
Content-Type: text/plain; charset=utf-8; format=flowed

I am trying to do something similar (apply ICP on a subset of a large 
dataset) and am running into confusion over the transformation matrix in 
the metadata.

The point cloud output of the ICP filter on the subset looks good. 
However when I try to use the resulting "transform" metadata as a input 
into filters.transformation.matrix over the full dataset, the resulting 
transformation moves the points wildly out of position.

Looking at the code, in IterativeClosestPoint.cpp, it looks like the 
transformation matrix (T) is applied after shifting the point cloud to 
the centroid of the reference point cloud (C), and the shifted back to 
regular coordinates.

So something like Pout = ((P - C) * T) + C

Where P is a 4 element input point vector (x,y,z,1), Pout is the output 
point vector, C is the centroid vector (x,y,z,0), T is the 4x4 
transformation matrix found by ICP.

T is reported in the metadata for the stage, however, C is not reported 
in the metadata.

But the transformation matrix that filters.transformation expects 
something more like

Pout = P * T'

If C is not (0,0,0) then T and T' will be different.

I modified IterativeClosestPoint.cpp to also export the centroid to the 
metadata and then created a pipeline with three transformations, first 
shifting by the negative of the centroid, then applying T, then shifting 
back by the centroid and that produced the expected results.

[abellgithub]

@chambbj : Thoughts?