Always provide a normalized result in pcl::transformPlane.

PointCloudLibrary · Oct 8, 2018 · 448e042 · 448e042
1 parent 46a544d
commit 448e042
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 1 deletion.
diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp
@@ -842,6 +842,11 @@ pcl::transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
   plane.coeffs () << plane_in;
   plane.transform (transformation);
   plane_out << plane.coeffs ();
+
+  // Versions prior to 3.3.2 don't normalize the result
+  #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
+    plane_out /= plane_out.template head<3> ().norm ();
+  #endif
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////
@@ -850,7 +855,8 @@ pcl::transformPlane (const pcl::ModelCoefficients::Ptr plane_in,
                            pcl::ModelCoefficients::Ptr plane_out,
                      const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
 {
-  Eigen::Matrix < Scalar, 4, 1 > v_plane_in (std::vector < Scalar > (plane_in->values.begin (), plane_in->values.end ()).data ());
+  std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
+  Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
   pcl::transformPlane (v_plane_in, v_plane_in, transformation);
   plane_out->values.resize (4);
   for (int i = 0; i < 4; i++)

diff --git a/test/common/test_eigen.cpp b/test/common/test_eigen.cpp
@@ -932,6 +932,8 @@ TEST (PCL, transformPlane)
   transformationd.linear() = (Eigen::Matrix3d) Eigen::AngleAxisd(M_PI/7, Eigen::Vector3d::UnitY())
   * Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ());
   test << 5.35315, 2.89914, 0.196848, -49.2788;
+  test /= test.head<3> ().norm ();
+
   tolerance = 1e-4;
 
   plane->values[0] = 5.4;