diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 408ee0a78c6..599b72fbd87 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -35,7 +35,6 @@ if(build) src/common.cpp src/correspondence.cpp src/distances.cpp - src/intersections.cpp src/parse.cpp src/poses_from_matches.cpp src/print.cpp @@ -117,6 +116,7 @@ if(build) include/pcl/common/impl/centroid.hpp include/pcl/common/impl/common.hpp include/pcl/common/impl/eigen.hpp + include/pcl/common/impl/intersections.hpp include/pcl/common/impl/copy_point.hpp include/pcl/common/impl/io.hpp include/pcl/common/impl/file_io.hpp diff --git a/common/src/intersections.cpp b/common/include/pcl/common/impl/intersections.hpp similarity index 67% rename from common/src/intersections.cpp rename to common/include/pcl/common/impl/intersections.hpp index 7473a3ed1c4..fdec367cbf2 100644 --- a/common/src/intersections.cpp +++ b/common/include/pcl/common/impl/intersections.hpp @@ -35,9 +35,14 @@ * */ -#include +#ifndef PCL_COMMON_INTERSECTIONS_IMPL_HPP_ +#define PCL_COMMON_INTERSECTIONS_IMPL_HPP_ + +#include #include +////////////////////////////////////////////////////////////////////////////////////////// + bool pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, @@ -67,62 +72,69 @@ pcl::lineWithLineIntersection (const pcl::ModelCoefficients &line_a, return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps)); } -bool -pcl::planeWithPlaneIntersection (const Eigen::Vector4f &plane_a, - const Eigen::Vector4f &plane_b, - Eigen::VectorXf &line, +template bool +pcl::planeWithPlaneIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + Eigen::Matrix &line, double angular_tolerance) { + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector5; + typedef Eigen::Matrix Matrix5; + //planes shouldn't be parallel - double test_cosine = plane_a.head<3>().dot(plane_b.head<3>()); + double test_cosine = plane_a.template head<3> ().dot (plane_b.template head<3> ()); double upper_limit = 1 + angular_tolerance; double lower_limit = 1 - angular_tolerance; if ((test_cosine < upper_limit) && (test_cosine > lower_limit)) { - PCL_ERROR ("Plane A and Plane B are parallel\n"); + PCL_DEBUG ("Plane A and Plane B are parallel\n"); return (false); } if ((test_cosine > -upper_limit) && (test_cosine < -lower_limit)) { - PCL_ERROR ("Plane A and Plane B are parallel\n"); + PCL_DEBUG ("Plane A and Plane B are parallel\n"); return (false); } - Eigen::Vector4f line_direction = plane_a.cross3(plane_b); + Vector4 line_direction = plane_a.cross3 (plane_b); line_direction.normalized(); //construct system of equations using lagrange multipliers with one objective function and two constraints - Eigen::MatrixXf langegrange_coefs(5,5); - langegrange_coefs << 2,0,0,plane_a[0],plane_b[0], 0,2,0,plane_a[1],plane_b[1], 0,0,2, plane_a[2], plane_b[2], plane_a[0], plane_a[1] , plane_a[2], 0,0, plane_b[0], plane_b[1], plane_b[2], 0,0; - - Eigen::VectorXf b; - b.resize(5); + Matrix5 langrange_coefs; + langrange_coefs << 2,0,0, plane_a[0], plane_b[0], + 0,2,0, plane_a[1], plane_b[1], + 0,0,2, plane_a[2], plane_b[2], + plane_a[0], plane_a[1], plane_a[2], 0, 0, + plane_b[0], plane_b[1], plane_b[2], 0, 0; + + Vector5 b; b << 0, 0, 0, -plane_a[3], -plane_b[3]; - //solve for the lagrange Multipliers - Eigen::VectorXf x; - x.resize(5); - x = langegrange_coefs.colPivHouseholderQr().solve(b); - line.resize(6); - line.head<3>() = x.head<3>(); // the x[3] and x[4] are the values of the lagrange multipliers and are neglected + //solve for the lagrange Multipliers + line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> (); line[3] = line_direction[0]; line[4] = line_direction[1]; line[5] = line_direction[2]; return true; -} +} -bool -pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a, - const Eigen::Vector4f &plane_b, - const Eigen::Vector4f &plane_c, - Eigen::Vector3f &intersection_point, +template bool +pcl::threePlanesIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + const Eigen::Matrix &plane_c, + Eigen::Matrix &intersection_point, double determinant_tolerance) { + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Matrix3; + + // TODO: Using Eigen::HyperPlanes is better to solve this problem // Check if some planes are parallel - Eigen::Matrix3f normals_in_lines; + Matrix3 normals_in_lines; for (int i = 0; i < 3; i++) { @@ -131,16 +143,16 @@ pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a, normals_in_lines (i, 2) = plane_c[i]; } - double determinant = normals_in_lines.determinant (); + Scalar determinant = normals_in_lines.determinant (); if (fabs (determinant) < determinant_tolerance) { // det ~= 0 - PCL_ERROR ("Two or more planes are parralel.\n"); + PCL_DEBUG ("At least two planes are parralel.\n"); return (false); } // Left part of the 3 equations - Eigen::Matrix3f left_member; + Matrix3 left_member; for (int i = 0; i < 3; i++) { @@ -150,7 +162,7 @@ pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a, } // Right side of the 3 equations - Eigen::Vector3f right_member; + Vector3 right_member; right_member << -plane_a[3], -plane_b[3], -plane_c[3]; // Solve the system @@ -158,3 +170,4 @@ pcl::threePlanesIntersection (const Eigen::Vector4f &plane_a, return (true); } +#endif //PCL_COMMON_INTERSECTIONS_IMPL_HPP diff --git a/common/include/pcl/common/intersections.h b/common/include/pcl/common/intersections.h index 1b195e116e2..32cff1b31ef 100644 --- a/common/include/pcl/common/intersections.h +++ b/common/include/pcl/common/intersections.h @@ -34,6 +34,7 @@ * $Id$ * */ + #ifndef PCL_INTERSECTIONS_H_ #define PCL_INTERSECTIONS_H_ @@ -57,10 +58,11 @@ namespace pcl * \param[in] sqr_eps maximum allowable squared distance to the true solution * \ingroup common */ - PCL_EXPORTS bool + PCL_EXPORTS inline bool lineWithLineIntersection (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, - Eigen::Vector4f &point, double sqr_eps = 1e-4); + Eigen::Vector4f &point, + double sqr_eps = 1e-4); /** \brief Get the intersection of a two 3D lines in space as a 3D point * \param[in] line_a the coefficients of the first line (point, direction) @@ -69,10 +71,12 @@ namespace pcl * \param[in] sqr_eps maximum allowable squared distance to the true solution * \ingroup common */ - PCL_EXPORTS bool + + PCL_EXPORTS inline bool lineWithLineIntersection (const pcl::ModelCoefficients &line_a, const pcl::ModelCoefficients &line_b, - Eigen::Vector4f &point, double sqr_eps = 1e-4); + Eigen::Vector4f &point, + double sqr_eps = 1e-4); /** \brief Determine the line of intersection of two non-parallel planes using lagrange multipliers * \note Described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA" @@ -81,11 +85,29 @@ namespace pcl * line.head<3>() the point on the line clossest to (0, 0, 0) * \return true if succeeded/planes aren't parallel */ - PCL_EXPORTS bool + PCL_EXPORTS template bool + planeWithPlaneIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + Eigen::Matrix &line, + double angular_tolerance = 0.1); + + PCL_EXPORTS inline bool planeWithPlaneIntersection (const Eigen::Vector4f &plane_a, - const Eigen::Vector4f &fplane_b, + const Eigen::Vector4f &plane_b, Eigen::VectorXf &line, - double angular_tolerance = 0.1); + double angular_tolerance = 0.1) + { + return (planeWithPlaneIntersection (plane_a, plane_b, line, angular_tolerance)); + } + + PCL_EXPORTS inline bool + planeWithPlaneIntersection (const Eigen::Vector4d &plane_a, + const Eigen::Vector4d &plane_b, + Eigen::VectorXd &line, + double angular_tolerance = 0.1) + { + return (planeWithPlaneIntersection (plane_a, plane_b, line, angular_tolerance)); + } /** \brief Determine the point of intersection of three non-parallel planes by solving the equations. * \note If using nearly parralel planes you can lower the determinant_tolerance value. This can @@ -98,15 +120,39 @@ namespace pcl * \param[out] intersection_point, the three coordinates x, y, z of the intersection point * \return true if succeeded/planes aren't parallel */ - PCL_EXPORTS bool - threePlanesIntersection (const Eigen::Vector4f &plane_a, + PCL_EXPORTS template bool + threePlanesIntersection (const Eigen::Matrix &plane_a, + const Eigen::Matrix &plane_b, + const Eigen::Matrix &plane_c, + Eigen::Matrix &intersection_point, + double determinant_tolerance = 1e-6); + + + PCL_EXPORTS inline bool + threePlanesIntersection (const Eigen::Vector4f &plane_a, const Eigen::Vector4f &plane_b, const Eigen::Vector4f &plane_c, Eigen::Vector3f &intersection_point, - double determinant_tolerance = 1e-6); + double determinant_tolerance = 1e-6) + { + return (threePlanesIntersection (plane_a, plane_b, plane_c, + intersection_point, determinant_tolerance)); + } + + PCL_EXPORTS inline bool + threePlanesIntersection (const Eigen::Vector4d &plane_a, + const Eigen::Vector4d &plane_b, + const Eigen::Vector4d &plane_c, + Eigen::Vector3d &intersection_point, + double determinant_tolerance = 1e-6) + { + return (threePlanesIntersection (plane_a, plane_b, plane_c, + intersection_point, determinant_tolerance)); + } } /*@}*/ -#endif //#ifndef PCL_INTERSECTIONS_H_ +#include +#endif //#ifndef PCL_INTERSECTIONS_H_ diff --git a/test/common/test_plane_intersection.cpp b/test/common/test_plane_intersection.cpp index 1cfe05738ff..c36fdb9d36d 100644 --- a/test/common/test_plane_intersection.cpp +++ b/test/common/test_plane_intersection.cpp @@ -45,123 +45,123 @@ using namespace pcl; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, lineWithLineIntersection) { - Eigen::VectorXf line_a(6); - Eigen::VectorXf line_b(6); + Eigen::VectorXf line_a (6); + Eigen::VectorXf line_b (6); //case 1 - line_a[0] = 0.01f; - line_a[1] = 0.02f; - line_a[2] = 0.03f; - line_a[3] = 0.4f; - line_a[4] = 0.5f; - line_a[5] = 0.6f; - - line_b[0] = 0.1f; - line_b[1] = 0.2f; - line_b[2] = 0.3f; - line_b[3] = 0.04f; - line_b[4] = 0.05f; - line_b[5] = 0.06f; + line_a[0] = 0.01; + line_a[1] = 0.02; + line_a[2] = 0.03; + line_a[3] = 0.4; + line_a[4] = 0.5; + line_a[5] = 0.6; + + line_b[0] = 0.1; + line_b[1] = 0.2; + line_b[2] = 0.3; + line_b[3] = 0.04; + line_b[4] = 0.05; + line_b[5] = 0.06; Eigen::Vector4f p1, p2; lineToLineSegment (line_a, line_b, p1, p2); Eigen::Vector4f point_case_1; - bool result_case_1 = lineWithLineIntersection(line_a, line_b, point_case_1); + bool result_case_1 = lineWithLineIntersection (line_a, line_b, point_case_1); double sqr_dist_case_1 = (p1 - p2).squaredNorm (); double default_sqr_eps = 1e-4; - EXPECT_GT(sqr_dist_case_1, default_sqr_eps); - Eigen::Vector4f zero(0.0f, 0.0f, 0.0f, 0.0f); + EXPECT_GT (sqr_dist_case_1, default_sqr_eps); + Eigen::Vector4f zero (0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(point_case_1[0], zero[0]); - EXPECT_EQ(point_case_1[1], zero[1]); - EXPECT_EQ(point_case_1[2], zero[2]); - EXPECT_EQ(point_case_1[3], zero[3]); + EXPECT_EQ (point_case_1[0], zero[0]); + EXPECT_EQ (point_case_1[1], zero[1]); + EXPECT_EQ (point_case_1[2], zero[2]); + EXPECT_EQ (point_case_1[3], zero[3]); - EXPECT_FALSE(result_case_1); + EXPECT_FALSE (result_case_1); pcl::ModelCoefficients line_a_mod; pcl::ModelCoefficients line_b_mod; std::vector values_a_case_1; - values_a_case_1.push_back(line_a[0]); - values_a_case_1.push_back(line_a[1]); - values_a_case_1.push_back(line_a[2]); - values_a_case_1.push_back(line_a[3]); - values_a_case_1.push_back(line_a[4]); - values_a_case_1.push_back(line_a[5]); + values_a_case_1.push_back (line_a[0]); + values_a_case_1.push_back (line_a[1]); + values_a_case_1.push_back (line_a[2]); + values_a_case_1.push_back (line_a[3]); + values_a_case_1.push_back (line_a[4]); + values_a_case_1.push_back (line_a[5]); std::vector values_b_case_1; - values_b_case_1.push_back(line_b[0]); - values_b_case_1.push_back(line_b[1]); - values_b_case_1.push_back(line_b[2]); - values_b_case_1.push_back(line_b[3]); - values_b_case_1.push_back(line_b[4]); - values_b_case_1.push_back(line_b[5]); + values_b_case_1.push_back (line_b[0]); + values_b_case_1.push_back (line_b[1]); + values_b_case_1.push_back (line_b[2]); + values_b_case_1.push_back (line_b[3]); + values_b_case_1.push_back (line_b[4]); + values_b_case_1.push_back (line_b[5]); line_a_mod.values = values_a_case_1; line_b_mod.values = values_b_case_1; Eigen::Vector4f point_mod_1; - EXPECT_FALSE(lineWithLineIntersection(line_a_mod, line_b_mod, point_mod_1)); - EXPECT_EQ(result_case_1, lineWithLineIntersection(line_a_mod, line_b_mod, point_mod_1)); + EXPECT_FALSE (lineWithLineIntersection (line_a_mod, line_b_mod, point_mod_1)); + EXPECT_EQ (result_case_1, lineWithLineIntersection (line_a_mod, line_b_mod, point_mod_1)); - EXPECT_EQ(point_mod_1[0], zero[0]); - EXPECT_EQ(point_mod_1[1], zero[1]); - EXPECT_EQ(point_mod_1[2], zero[2]); - EXPECT_EQ(point_mod_1[3], zero[3]); + EXPECT_EQ (point_mod_1[0], zero[0]); + EXPECT_EQ (point_mod_1[1], zero[1]); + EXPECT_EQ (point_mod_1[2], zero[2]); + EXPECT_EQ (point_mod_1[3], zero[3]); //case 2 - line_a[0] = 0.00100f; - line_a[1] = 0.00200f; - line_a[2] = 0.00300f; - line_a[3] = 0.00400f; - line_a[4] = 0.00500f; - line_a[5] = 0.00600f; - - line_b[0] = 0.00157f; - line_b[1] = 0.00233f; - line_b[2] = 0.00378f; - line_b[3] = 0.00495f; - line_b[4] = 0.00565f; - line_b[5] = 0.00666f; + line_a[0] = 0.00100; + line_a[1] = 0.00200; + line_a[2] = 0.00300; + line_a[3] = 0.00400; + line_a[4] = 0.00500; + line_a[5] = 0.00600; + + line_b[0] = 0.00157; + line_b[1] = 0.00233; + line_b[2] = 0.00378; + line_b[3] = 0.00495; + line_b[4] = 0.00565; + line_b[5] = 0.00666; lineToLineSegment (line_a, line_b, p1, p2); Eigen::Vector4f point_case_2; double sqr_eps_case_2 = 1e-1; - bool result_case_2 = lineWithLineIntersection(line_a, line_b, point_case_2, sqr_eps_case_2); + bool result_case_2 = lineWithLineIntersection (line_a, line_b, point_case_2, sqr_eps_case_2); double sqr_dist_case_2 = (p1 - p2).squaredNorm (); - EXPECT_LT(sqr_dist_case_2, sqr_eps_case_2); + EXPECT_LT (sqr_dist_case_2, sqr_eps_case_2); - EXPECT_EQ(point_case_2[0], p1[0]); - EXPECT_EQ(point_case_2[1], p1[1]); - EXPECT_EQ(point_case_2[2], p1[2]); - EXPECT_EQ(point_case_2[3], p1[3]); + EXPECT_EQ (point_case_2[0], p1[0]); + EXPECT_EQ (point_case_2[1], p1[1]); + EXPECT_EQ (point_case_2[2], p1[2]); + EXPECT_EQ (point_case_2[3], p1[3]); - EXPECT_TRUE(result_case_2); + EXPECT_TRUE (result_case_2); pcl::ModelCoefficients line_a_mod_2; pcl::ModelCoefficients line_b_mod_2; std::vector values_a_case_2; - values_a_case_2.push_back(0.1000f); - values_a_case_2.push_back(0.2000f); - values_a_case_2.push_back(0.3000f); - values_a_case_2.push_back(0.4000f); - values_a_case_2.push_back(0.5000f); - values_a_case_2.push_back(0.6000f); + values_a_case_2.push_back (0.1000); + values_a_case_2.push_back (0.2000); + values_a_case_2.push_back (0.3000); + values_a_case_2.push_back (0.4000); + values_a_case_2.push_back (0.5000); + values_a_case_2.push_back (0.6000); std::vector values_b_case_2; - values_b_case_2.push_back(0.1001f); - values_b_case_2.push_back(0.2001f); - values_b_case_2.push_back(0.3001f); - values_b_case_2.push_back(0.4001f); - values_b_case_2.push_back(0.5001f); - values_b_case_2.push_back(0.6001f); + values_b_case_2.push_back (0.1001); + values_b_case_2.push_back (0.2001); + values_b_case_2.push_back (0.3001); + values_b_case_2.push_back (0.4001); + values_b_case_2.push_back (0.5001); + values_b_case_2.push_back (0.6001); line_a_mod_2.values = values_a_case_2; line_b_mod_2.values = values_b_case_2; @@ -177,166 +177,165 @@ TEST (PCL, lineWithLineIntersection) lineToLineSegment (coeff1, coeff2, p1_mod, p2_mod); double sqr_mod_act_2 = (p1_mod - p2_mod).squaredNorm (); - EXPECT_LT(sqr_mod_act_2, sqr_mod_case_2); - EXPECT_EQ(lineWithLineIntersection (coeff1, coeff2, point_mod_case_2, sqr_mod_case_2), - lineWithLineIntersection(line_a_mod_2, line_b_mod_2, point_mod_2, sqr_mod_case_2)); - EXPECT_TRUE(lineWithLineIntersection(line_a_mod_2, line_b_mod_2, point_mod_2, sqr_mod_case_2)); + EXPECT_LT (sqr_mod_act_2, sqr_mod_case_2); + EXPECT_EQ (lineWithLineIntersection (coeff1, coeff2, point_mod_case_2, sqr_mod_case_2), + lineWithLineIntersection (line_a_mod_2, line_b_mod_2, point_mod_2, sqr_mod_case_2)); + EXPECT_TRUE (lineWithLineIntersection (line_a_mod_2, line_b_mod_2, point_mod_2, sqr_mod_case_2)); - EXPECT_EQ(point_mod_2[0], point_mod_case_2[0]); - EXPECT_EQ(point_mod_2[1], point_mod_case_2[1]); - EXPECT_EQ(point_mod_2[2], point_mod_case_2[2]); - EXPECT_EQ(point_mod_2[3], point_mod_case_2[3]); + EXPECT_EQ (point_mod_2[0], point_mod_case_2[0]); + EXPECT_EQ (point_mod_2[1], point_mod_case_2[1]); + EXPECT_EQ (point_mod_2[2], point_mod_case_2[2]); + EXPECT_EQ (point_mod_2[3], point_mod_case_2[3]); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, planeWithPlaneIntersection) { - //Testing parallel planes + // Testing parallel planes const int k = 2; - float x = 1.0f; - float y = 2.0f; - float z = 3.0f; - float w = 4.0f; - Eigen::Vector4f plane_a; - plane_a.x() = x; - plane_a.y() = y; - plane_a.z() = z; - plane_a.w() = w; - - EXPECT_EQ(1.0f, plane_a.x()); - EXPECT_EQ(2.0f, plane_a.y()); - EXPECT_EQ(3.0f, plane_a.z()); - EXPECT_EQ(4.0f, plane_a.w()); - - Eigen::Vector4f plane_b; - plane_b.x() = x; - plane_b.y() = y; - plane_b.z() = z; - plane_b.w() = w + k; - - EXPECT_EQ(1.0f, plane_b.x()); - EXPECT_EQ(2.0f, plane_b.y()); - EXPECT_EQ(3.0f, plane_b.z()); - EXPECT_EQ(6.0f, plane_b.w()); - - Eigen::VectorXf line; - - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 45)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 90)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 180)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 360)); - - plane_b.x() = k * x; - plane_b.y() = k * y; - plane_b.z() = k * z; - plane_b.w() = k * w; - - EXPECT_EQ(2.0f, plane_b.x()); - EXPECT_EQ(4.0f, plane_b.y()); - EXPECT_EQ(6.0f, plane_b.z()); - EXPECT_EQ(8.0f, plane_b.w()); - - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 45)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 90)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 180)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 360)); - - //overlapping planes + double x = 1.0; + double y = 2.0; + double z = 3.0; + double w = 4.0; + float xf = 1.0; + float yf = 2.0; + float zf = 3.0; + float wf = 4.0; + + Eigen::Vector4d plane_a, plane_b; + plane_a << x, y, z, w; + plane_b << x, y, z, (w+k); + + Eigen::Vector4f plane_af, plane_bf; + plane_af << xf, yf, zf, wf; + plane_bf << xf, yf, zf, (wf+k); + + Eigen::VectorXd line; + Eigen::VectorXf linef; + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 45)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 45)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 90)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 90)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 180)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 180)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 360)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 360)); + + plane_b << x, y, z, w; + plane_b *= k; + plane_bf << xf, yf, zf, wf; + plane_bf *= k; + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 45)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 45)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 90)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 90)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 180)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 180)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 360)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 360)); + + // Overlapping planes plane_b.w() = w; - EXPECT_EQ(4.0f, plane_b.w()); - - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 45)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 90)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 180)); - std::cout << std::endl; - EXPECT_FALSE(planeWithPlaneIntersection(plane_a, plane_b, line, 360)); - - //orthogonal planes - plane_a.x() = 2.0f; - plane_a.y() = 1.0f; - plane_a.z() = -5.0f; - plane_a.w() = 6.0f; - EXPECT_EQ(2.0f, plane_a.x()); - EXPECT_EQ(1.0f, plane_a.y()); - EXPECT_EQ(-5.0f, plane_a.z()); - EXPECT_EQ(6.0f, plane_a.w()); - - plane_b.x() = 2.0f; - plane_b.y() = 1.0f; - plane_b.z() = 1.0f; - plane_b.w() = 7.0f; - - EXPECT_EQ(2.0f, plane_b.x()); - EXPECT_EQ(1.0f, plane_b.y()); - EXPECT_EQ(1.0f, plane_b.z()); - EXPECT_EQ(7.0f, plane_b.w()); - - std::cout << std::endl; - EXPECT_TRUE(planeWithPlaneIntersection(plane_a, plane_b, line, 0)); - - //general planes - plane_a.x() = 1.555f; - plane_a.y() = 0.894f; - plane_a.z() = 1.234f; - plane_a.w() = 3.567f; - - plane_b.x() = 0.743f; - plane_b.y() = 1.890f; - plane_b.z() = 6.789f; - plane_b.w() = 5.432f; - - std::cout << std::endl; - EXPECT_TRUE(planeWithPlaneIntersection(plane_a, plane_b, line, 0)); + plane_bf.w() = wf; + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 45)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 45)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 90)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 90)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 180)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 180)); + + EXPECT_FALSE (planeWithPlaneIntersection (plane_a, plane_b, line, 360)); + EXPECT_FALSE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 360)); + + // Orthogonal planes + plane_a << 2.0, 1.0, -5.0, 6.0; + plane_b << 2.0, 1.0, 1.0, 7.0; + plane_af << 2.0, 1.0, -5.0, 6.0; + plane_bf << 2.0, 1.0, 1.0, 7.0; + EXPECT_TRUE (planeWithPlaneIntersection (plane_a, plane_b, line, 0)); + EXPECT_TRUE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 0)); + + // General planes + plane_a << 1.555, 0.894, 1.234, 3.567; + plane_b << 0.743, 1.890, 6.789, 5.432; + plane_af << 1.555, 0.894, 1.234, 3.567; + plane_bf << 0.743, 1.890, 6.789, 5.432; + + EXPECT_TRUE (planeWithPlaneIntersection (plane_a, plane_b, line, 0)); + EXPECT_TRUE (planeWithPlaneIntersection (plane_af, plane_bf, linef, 0)); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, threePlanesIntersection) { + // Testing with floats/doubles // Testing 2 parallel planes - Eigen::Vector4f plane_a (1.0f, 0.0f, 0.0f, -0.5f); - Eigen::Vector4f plane_b (1.0f, 0.0f, 0.0f, 0.5f); - Eigen::Vector4f plane_c (0.0f, 0.0f, 1.0f, -0.5f); - Eigen::Vector3f point (1.0f, 2.0f, 3.0f); + Eigen::Vector4d plane_a (1.0, 0.0, 0.0, -0.5); + Eigen::Vector4d plane_b (1.0, 0.0, 0.0, 0.5); + Eigen::Vector4d plane_c (0.0, 0.0, 1.0, -0.5); + Eigen::Vector3d point (1.0, 2.0, 3.0); + + Eigen::Vector4f fplane_a (1.0, 0.0, 0.0, -0.5); + Eigen::Vector4f fplane_b (1.0, 0.0, 0.0, 0.5); + Eigen::Vector4f fplane_c (0.0, 0.0, 1.0, -0.5); + Eigen::Vector3f fpoint (1.0, 2.0, 3.0); - std::cout << std::endl; EXPECT_FALSE (threePlanesIntersection (plane_a, plane_b, plane_c, point, 1e-6)); - std::cout << std::endl; + EXPECT_FALSE (threePlanesIntersection (fplane_a, fplane_b, fplane_c, fpoint, 1e-6)); EXPECT_FALSE (threePlanesIntersection (plane_a, plane_b, plane_c, point, 1e-3)); - EXPECT_FLOAT_EQ (1.0f, point (0)); - EXPECT_FLOAT_EQ (2.0f, point (1)); - EXPECT_FLOAT_EQ (3.0f, point (2)); + EXPECT_FALSE (threePlanesIntersection (fplane_a, fplane_b, fplane_c, fpoint, 1e-3)); - //perfect box - plane_b << 0.0f, 1.0f, 0.0f, 0.5f; + EXPECT_DOUBLE_EQ (1.0, point [0]); + EXPECT_DOUBLE_EQ (2.0, point [1]); + EXPECT_DOUBLE_EQ (3.0, point [2]); + EXPECT_FLOAT_EQ (1.0, fpoint [0]); + EXPECT_FLOAT_EQ (2.0, fpoint [1]); + EXPECT_FLOAT_EQ (3.0, fpoint [2]); - std::cout << std::endl; - EXPECT_TRUE (threePlanesIntersection (plane_a, plane_b, plane_c, point)); - EXPECT_FLOAT_EQ (0.5f, point (0)); - EXPECT_FLOAT_EQ (-0.5f, point (1)); - EXPECT_FLOAT_EQ (0.5f, point (2)); + // Perfect box + plane_b << 0.0, 1.0, 0.0, 0.5; + fplane_b << 0.0, 1.0, 0.0, 0.5; - //general planes - plane_a << 1.4564f, 0.5465f, -0.1325f, 0.4685f; - plane_b << -1.5619f, 5.5461f, 5.4569f, 2.9414f; - plane_c << 0.9852f, 654.55f, -0.1546f, -45.1516f; + EXPECT_TRUE (threePlanesIntersection (plane_a, plane_b, plane_c, point)); + EXPECT_TRUE (threePlanesIntersection (fplane_a, fplane_b, fplane_c, fpoint)); + + EXPECT_DOUBLE_EQ (0.5, point [0]); + EXPECT_DOUBLE_EQ (-0.5, point [1]); + EXPECT_DOUBLE_EQ (0.5, point [2]); + EXPECT_FLOAT_EQ (0.5, fpoint [0]); + EXPECT_FLOAT_EQ (-0.5, fpoint [1]); + EXPECT_FLOAT_EQ (0.5, fpoint [2]); + + // Random planes + plane_a << 1.4564, 0.5465, -0.1325, 0.4685; + plane_b << -1.5619, 5.5461, 5.4569, 2.9414; + plane_c << 0.9852, 654.55, -0.1546, -45.1516; + fplane_a << 1.4564, 0.5465, -0.1325, 0.4685; + fplane_b << -1.5619, 5.5461, 5.4569, 2.9414; + fplane_c << 0.9852, 654.55, -0.1546, -45.1516; - std::cout << std::endl; EXPECT_TRUE (threePlanesIntersection (plane_a, plane_b, plane_c, point)); - EXPECT_NEAR (-0.413977f, point (0), 1e-4); - EXPECT_NEAR (0.0694323f, point (1), 1e-4); - EXPECT_NEAR (-0.728082f, point (2), 1e-4); + EXPECT_TRUE (threePlanesIntersection (fplane_a, fplane_b, fplane_c, fpoint)); + + EXPECT_NEAR (-0.413977, point [0], 1e-4); + EXPECT_NEAR (0.0694323, point [1], 1e-4); + EXPECT_NEAR (-0.728082, point [2], 1e-4); + EXPECT_NEAR (-0.413977, fpoint [0], 1e-4); + EXPECT_NEAR (0.0694323, fpoint [1], 1e-4); + EXPECT_NEAR (-0.728082, fpoint [2], 1e-4); } //* ---[ */