PointCloudLibrary / PointCloudLibrary/pcl
bug in function pcl::transformPointCloudWithNormals
@taketwo is already working on this.
Since Oct 7, 2019.
- 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
First steps
- Read the whole issue, then the project's contributing guide.
- Comment on the issue to say you are picking it up — it saves two people doing the same work.
- Fork the repository and make your change on a branch.
- Open a pull request that references the issue number.
Assessment
This issue has not been assessed yet.