PointCloudLibrary / PointCloudLibrary/pcl

bug in function pcl::transformPointCloudWithNormals

Open
#3,395 6 comments 0 reactions 1 assignee View on GitHub

@taketwo is already working on this.

Since Oct 7, 2019.

kind: bug kind: todo module: common needs: testing
Dominant language
C++
Stars
11.1k
Forks
4.7k
Avg merge
4d 10h
Merged PRs (30d)
6

Description

Context

is this code are valid? (file common/include/pcl/common/impl/transforms.hpp)

/** Apply SO3 transform (top-left corner of the transform matrix).
  * \param[in] src input 3D point (pointer to 3 floats)
  * \param[out] tgt output 3D point (pointer to 4 floats), can be the same as input. The fourth element is set to 0. */
void so3 (const float* src, float* tgt) const
{
  const Scalar p[3] = { src[0], src[1], src[2] };  // need this when src == tgt
  tgt[0] = static_cast<float> (tf (0, 0) * p[0] + tf (0, 1) * p[1] + tf (0, 2) * p[2]);
  tgt[1] = static_cast<float> (tf (1, 0) * p[0] + tf (1, 1) * p[1] + tf (1, 2) * p[2]);
  tgt[2] = static_cast<float> (tf (2, 0) * p[0] + tf (2, 1) * p[1] + tf (2, 2) * p[2]);
  tgt[3] = 0;
}

If the transformer contains scale, then upper left 3x3 matrix are not only rotation, but rotation and scale.

I have pcl::PointNormal with:
xyz = [3.389206 -6.166714 2.052866]
normal = [-0.387280 -0.007470 -0.921932]

And i have transform matrix:

-126.670  -114.938     56.337   390568.258
-104.939   138.633     46.888  6650131.285
 -73.296    0.1521   -164.492      610.378
     0.0       0.0        0.0          1.0

Expected Behavior

After transformation i want to see this point:
xyz = [390963.375 6649017 23.34]
normal = [-0.011235 -0.020115 0.999735]

Current Behavior

However, current version of PCL create this point:
xyz = [390963.375 6649017 23.34]
normal = [-2.02353862, -3.62236021, 180.03537724]

Code to Reproduce

Eigen::Vector3f xyz, norm;
Eigen::Matrix4f m;
xyz << 3.389206, -6.166714, 2.052866;
norm << -0.387280, -0.007470, -0.921932;
m << -126.670,  -114.938,     56.337,   390568.258,
     -104.939,   138.633,     46.888,  6650131.285,
      -73.296,    0.1521,   -164.492,      610.378,
         0.0,       0.0,         0.0,          1.0;

pcl::PointNormal p;
p.getVector3fMap() = xyz;
p.getNormalVector3fMap() = norm;

pcl::PointCloud<pcl::PointNormal> cloud;
cloud.push_back(p);

pcl::transformPointCloudWithNormals(cloud, cloud, m);

std::cout << cloud[0];

Possible Solution

in older version of function pcl::transformPointCloudWithNormals was this code:

// Rotate normals (WARNING: transform.rotation () uses SVD internally!)
//cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();

after i uncomment this part of code, and comment 3 line below (with custom transform, like in function so3), behavior of function pcl::transformPointCloudWithNormals has become valid.

Contributor guide

Open the contributing guide

First steps

  1. Read the whole issue, then the project's contributing guide.
  2. Comment on the issue to say you are picking it up — it saves two people doing the same work.
  3. Fork the repository and make your change on a branch.
  4. Open a pull request that references the issue number.

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.