[Registration] gauss_c2 calculation formula in NormalDistributionsTransform

[KotaYonezawa]

Describe the bug

Thanks a lot for great library. I found a suspicious code in ndt.hpp to calculate "gauss_c2" value. But I'm not sure that what is the correct answer, so I would like to hear professional member's opinion...

Context

Following code is in ndt.hpp to calculate NDT related parameters.

  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
  const double gauss_c1 = 10.0 * (1 - outlier_ratio_);
  const double gauss_c2 = outlier_ratio_ / pow(resolution_, 3); // *** HERE ***
  const double gauss_d3 = -std::log(gauss_c2);
  gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
  gauss_d2_ =
      -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
                    gauss_d1_);

But I wondered why resolution^3 is used to calculate gauss_c2. For example, if resolution value is small, gauss_c2 can become very big value, and NDT tends to work incorrectly. Several paper pointed out that the user need to decide resolution carefully, but I supposed that this code may be one of the cause of difficulty to decide best resolution parameter. I checked Dr. M.Magnusson's paper, but I think this calculation formula is not written clearly, I supposed.

I found a following code, seems to be for D2D (Distribution-to-Distribution) NDT paper, Dr. M.Magnusson is one of the author. https://gitsvn-nt.oru.se/software/ndt_core_public/-/blob/master/ndt_registration/src/ndt_matcher_p2d.cpp

On this code, resolution is not used to calculate gauss_c2.

    support_size = 4; //???
    ...
    lfc2 = outlier_ratio/pow(support_size,3);

On this code, "support_size" parameter is used instead of resolution, and support_size = 4 fixed value.

I tentatively checked using NDT tutorial (https://pointclouds.org/documentation/tutorials/normal_distributions_transform.html) using following parameter. (Note that resolution is small (0.5m = 50cm))

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.000001);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (1.0);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (0.5);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);

Current PCL code. (gauss_c2 = outlierratio / pow(resolution_, 3);) NDT matching failed.

Using modified code. (gauss_c2 = outlierratio / pow(4.0, 3);) NDT matching succeeded.

I'm not sure that above code (gauss_c2 = outlierratio / pow(4.0, 3);) is best and correct. But I think it's better than current PCL code. (gauss_c2 = outlierratio / pow(resolution_, 3);)

What do you think? Again, I would like to hear professional member's opinion to improve this point...

Your Environment (please complete the following information):

OS: Windows10 Compiler: Compiler: Visual Studio 2019 Build Tools PCL Version: HEAD

[mvieth]

Great that you investigate the NormalDistributionsTransform so thoroughly and report things to fix and improve! I looked into this a bit: Magnusson's thesis says: "The constants c1 and c2 can be determined by requiring that the probability mass of p(x) equals one within the space spanned by a cell" (a referenced paper, "A Probabilistic Framework for Robust and Accurate Matching of Point Clouds" by Biber, Fleck, Strasser says pretty much the same). This could be a hint why the constants are computed this way. I don't see why choosing the resolution/support size as 4 would be a good fit for all cases/datasets? Apart from the resolution, the outlier ratio also influences the parameters c1 and c2. Did you try to change that? Maybe the default outlier ratio is not a good value for that dataset? @grischi you opened the pull request about renaming a variable in ndt, so I wondered if you have any experience or insights into this matter?

[grischi]

The resolution^3 are the result of the process @mvieth cites from Magnusson's thesis. The constant c_2 is related to the uniformly distributed part of $\bar p$ (Eq. 6.7). The probability mass is calculated by an integration of the distribution over the voxel size. If we consider the uniform part only, integration gives resolution^3 * c_2. In order to have a probability mass equal to one, c_2 must be 1/resolution^3.

[KotaYonezawa]

Thanks a lot for comments.

Apart from the resolution, the outlier ratio also influences the parameters c1 and c2. Did you try to change that? Maybe the default outlier ratio is not a good value for that dataset?

Understood. I'm not sure to decide "correct" outlier ratio, so I tried resolution = 1.0 case.

On current PCL code, (gauss_c2 = outlierratio / pow(resolution_, 3);) resolution = 1.0 case with default outlier ratio, matching failed too...

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.000001);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (1.0);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);

If gauss_c2 calculation code is modified, (gauss_c2 = outlierratio / pow(4.0, 3);) matching was OK.

So I changed outlier ratio from default (0.55) to 0.3. On this case, both (gauss_c2 = outlierratio / pow(resolution_, 3);) and (gauss_c2 = outlierratio / pow(4.0, 3);) are OK on resolution = 1.0.

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.000001);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (1.0);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);
  // Outlier ratio.
  ndt.setOulierRatio (0.30);

Then, I tried resolution = 0.5 again with outlier ratio = 0.3.

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.000001);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (1.0);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (0.5);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);
  // Outlier ratio.
  ndt.setOulierRatio (0.30);

On current PCL code, (gauss_c2 = outlierratio / pow(resolution_, 3);), matching failed.

On modified, (gauss_c2 = outlierratio / pow(4.0, 3);) matching was OK.

[mvieth]

@grischi Thank you, that makes a lot of sense. Applying that logic to the computation of the c1 parameter, would the integral of the (unscaled) 3D normal distribution then be assumed as 0.1? But why? Shouldn't that depend on the covariance matrix and also the resolution (the smaller the resolution, the less of the distribution tails are integrated)? @KotaYonezawa So with the current code, the smaller you want the resolution to be, the smaller you have to set the outlier ratio to counteract that. It is still unclear whether 0.3 is a good outlier ratio for the dataset. In the project you linked above, the outlier ratio is even set to 0.01 (no idea what dataset is used). An interesting experiment would be to generate some kind of dataset with a known outlier ratio (outliers = basically random points?), then test ndt with different resolutions

[grischi]

Right @mvieth, the scale factor should be 1/sqrt((2*pi)^3*det(Sigma)). I however don't see a direct dependance on the resolution, but Sigma indirectly depends on it as points outside a voxel are not considered when computing Sigma for that voxel.

Sigma takes a different value for each voxel. It could be that this part of c1 is considered at another point in the code. I hope to find some time for further investigation on thursday.

[KotaYonezawa]

@mvieth Thanks a lot for comment! Yes, as you said, on current code, the user need to optimize both resolution and outlier ratio. I felt it's not good to use... I think resolution and outlier ratio should be controlled separately, basically.

On current test using NDT tutorial (https://pointclouds.org/documentation/tutorials/normal_distributions_transform.html) if we changed the outlier ratio to 0.001, both (gauss_c2 = outlierratio / pow(resolution_, 3);) and (gauss_c2 = outlierratio / pow(4.0, 3);) are OK with resolution = 0.5. (If outlier ratio = 0.01, (gauss_c2 = outlierratio / pow(resolution_, 3); matching was failed.)

I do not have another good data set can be used here now, so I'll consider and share if I can get interesting result.

[KotaYonezawa]

I made test point cloud data from "Stanford Bunny". https://casual-effects.com/data/ Scaled x 10 (after that, about 16m height bunny) and shift 1m to X and Y direction, and rotate 10 degree around Z axis for source data.

And then made point cloud using "CloudCompare" tool. (Density = 100.0)

I replaced pcd file name based on NDT tutorial code. (https://pointclouds.org/documentation/tutorials/normal_distributions_transform.html) And changed init_guess = Eigen::Matrix4f::Identity () Other parameters are:

  ndt.setTransformationEpsilon (0.000001);
  ndt.setStepSize (1.0);
  ndt.setMaximumIterations (35);

Parameter	Current Code	Modified(gauss_c2)
outlier ratio = 0.55(default), resolution = 2.0	OK	OK
outlier ratio = 0.55(default), resolution = 1.0	OK	OK
outlier ratio = 0.55(default), resolution = 0.5	OK	OK
outlier ratio = 0.55(default), resolution = 0.3	NG (1)	OK (2)
outlier ratio = 0.01, resolution = 0.3	OK	OK

(1) screen shot

(2) screen shot

bunny_pcddata.zip

[grischi]

jfyi: i'm working on an improvement, will create a PR when done. Validation will however cost some time.

[Kin-Zhang]

ndt.setTransformationEpsilon (0.000001);
ndt.setStepSize (1.0);
ndt.setMaximumIterations (35);

Parameter Current Code Modified(gauss_c2) outlier ratio = 0.55(default), resolution = 2.0 OK OK outlier ratio = 0.55(default), resolution = 1.0 OK OK outlier ratio = 0.55(default), resolution = 0.5 OK OK outlier ratio = 0.55(default), resolution = 0.3 NG (1) OK (2) outlier ratio = 0.01, resolution = 0.3 OK OK

I think the problem maybe is your stepsize is too big.... for resolution 0.3... normally we will set the stepsize as resolution/10

[KotaYonezawa]

@Kin-Zhang Thanks for comment.

I think the problem maybe is your stepsize is too big.... for resolution 0.3... normally we will set the stepsize as resolution/10

I tentatively tried to change stepsize to small value. Please check following. Even if I used small step size, current code (gauss_c2 = outlierratio / pow(resolution_, 3);) still not OK. Using modified code (gauss_c2 = outlierratio / pow(4.0, 3);), stepsize = 0.03 seems too small but stepsize = 0.1 was OK.

Parameter	Current Code	Modified(gauss_c2)
outlier ratio = 0.55(default), resolution = 0.3, stepsize = 1.0 (previous result)	NG	OK
outlier ratio = 0.55(default), resolution = 0.3, stepsize = 0.03	NG (1)	NG (2)
outlier ratio = 0.55(default), resolution = 0.3, stepsize = 0.1	NG (3)	OK (4)

(1) screen shot

(2) screen shot

(3) screen shot

(4) screen shot

[Kin-Zhang]

@KotaYonezawa Did you also set your Transformation to this one?

  ndt.setTransformationEpsilon (0.000001);

[KotaYonezawa]

@Kin-Zhang

Did you also set your Transformation to this one?

Yes, this value was same as previous test.