Skip to content

Regression fix for commit #646 #650

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Apr 24, 2014
Merged

Regression fix for commit #646 #650

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 16 additions & 17 deletions common/include/pcl/common/impl/intersections.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,31 +78,32 @@ pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
double angular_tolerance)
{
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 5, 1> Vector5;
typedef Eigen::Matrix<Scalar, 5, 3> Matrix5;
typedef Eigen::Matrix<Scalar, 5, 5> Matrix5;

// Normalize plane normals
Vector3 plane_a_norm (plane_a.template head<3> ());
Vector3 plane_b_norm (plane_b.template head<3> ());
plane_a_norm.normalize ();
plane_b_norm.normalize ();

//planes shouldn't be parallel
double test_cosine = plane_a.template head<3> ().dot (plane_b.template head<3> ());
// Test if planes are parallel (test_cos == 1)
double test_cos = plane_a_norm.dot (plane_b_norm);
double upper_limit = 1 + angular_tolerance;
double lower_limit = 1 - angular_tolerance;

if ((test_cosine < upper_limit) && (test_cosine > lower_limit))
if ((test_cos > lower_limit) && (test_cos < upper_limit))
{
PCL_DEBUG ("Plane A and Plane B are parallel\n");
return (false);
}

if ((test_cosine > -upper_limit) && (test_cosine < -lower_limit))
{
PCL_DEBUG ("Plane A and Plane B are parallel\n");
PCL_DEBUG ("Plane A and Plane B are parallel.\n");
return (false);
}

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
// Construct system of equations using lagrange multipliers with one objective function and two constraints
Matrix5 langrange_coefs;
langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
0,2,0, plane_a[1], plane_b[1],
Expand All @@ -114,12 +115,10 @@ pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
b << 0, 0, 0, -plane_a[3], -plane_b[3];

line.resize(6);
//solve for the lagrange Multipliers
// 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;
line.template tail<3>() = line_direction.template head<3>();
return (true);
}

template <typename Scalar> bool
Expand Down
3 changes: 2 additions & 1 deletion common/include/pcl/common/intersections.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,9 @@ namespace pcl
* \note Described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA"
* \param[in] plane_a coefficients of plane A and plane B in the form ax + by + cz + d = 0
* \param[out] plane_b coefficients of line where line.tail<3>() = direction vector and
* line.head<3>() the point on the line clossest to (0, 0, 0)
* line.head<3>() the point on the line closest to (0, 0, 0)
* \return true if succeeded/planes aren't parallel
* \note Plane normal doesn't have to be normalized
*/
PCL_EXPORTS template <typename Scalar> bool
planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
Expand Down
168 changes: 84 additions & 84 deletions test/common/test_plane_intersection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -192,90 +192,90 @@ TEST (PCL, lineWithLineIntersection)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (PCL, planeWithPlaneIntersection)
{
// Testing parallel planes
const int k = 2;
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;
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));
Eigen::Vector4f plane_a, plane_b;
Eigen::Vector4d plane_ad, plane_bd;
Eigen::VectorXf line;
Eigen::VectorXd lined;

// Testing parallel planes, plane normal not normalized!
plane_a << 1.0, 2.0, 3.0, 0.0;
plane_b << plane_a;
plane_b[3] += 1.0;
plane_ad << 1.0, 2.0, 3.0, 0.0;
plane_bd << plane_ad;
plane_bd[3] -= -1.0;

EXPECT_FALSE (pcl::planeWithPlaneIntersection (plane_a, plane_b, line, 1e-6));
EXPECT_FALSE (pcl::planeWithPlaneIntersection (plane_ad, plane_bd, lined, 1e-6));

// Testing nearly parallel planes, plane normal not normalized!
plane_a << 1.0, 2.0, 3.0, -0.5;
plane_b << 1.0, 2.5, 3.0, 0.5;;
plane_ad << 1.0, 2.0, 3.0, -0.5;
plane_bd << 1.0, 2.5, 3.0, 0.5;

EXPECT_TRUE (pcl::planeWithPlaneIntersection (plane_a, plane_b, line, 1e-3));
EXPECT_TRUE (pcl::planeWithPlaneIntersection (plane_ad, plane_bd, lined, 1e-3));

double tolerance = 1e-4;
EXPECT_NEAR (line[0], 0.45, tolerance);
EXPECT_NEAR (line[1], -2.0, tolerance);
EXPECT_NEAR (line[2], 1.35, tolerance);
EXPECT_NEAR (line[3], -1.5, tolerance);
EXPECT_NEAR (line[4], 0.0, tolerance);
EXPECT_NEAR (line[5], 0.5, tolerance);
EXPECT_NEAR (lined[0], 0.45, tolerance);
EXPECT_NEAR (lined[1], -2.0, tolerance);
EXPECT_NEAR (lined[2], 1.35, tolerance);
EXPECT_NEAR (lined[3], -1.5, tolerance);
EXPECT_NEAR (lined[4], 0.0, tolerance);
EXPECT_NEAR (lined[5], 0.5, tolerance);

// Orthogonal planes, Hessian form (plane normal normalized)
plane_a << 0.0, 0.0, 1.0, +0.0;
plane_b << 1.0, 0.0, 0.0, -0.5;
plane_ad << 0.0, 0.0, 1.0, +0.0;
plane_bd << 1.0, 0.0, 0.0, -0.5;

EXPECT_TRUE (pcl::planeWithPlaneIntersection (plane_a, plane_b, line, 0.1));
EXPECT_TRUE (pcl::planeWithPlaneIntersection (plane_ad, plane_bd, lined, 0.1));

tolerance = 1e-5;
EXPECT_NEAR (line[0], 0.5, tolerance);
EXPECT_NEAR (line[1], 0.0, tolerance);
EXPECT_NEAR (line[2], 0.0, tolerance);
EXPECT_NEAR (line[3], 0.0, tolerance);
EXPECT_NEAR (line[4], 1.0, tolerance);
EXPECT_NEAR (line[5], 0.0, tolerance);
EXPECT_NEAR (lined[0], 0.5, tolerance);
EXPECT_NEAR (lined[1], 0.0, tolerance);
EXPECT_NEAR (lined[2], 0.0, tolerance);
EXPECT_NEAR (lined[3], 0.0, tolerance);
EXPECT_NEAR (lined[4], 1.0, tolerance);
EXPECT_NEAR (lined[5], 0.0, tolerance);

// Random planes, plane normal not normalized!
plane_a << 24.234, -22.234, 3.0823, -24.5;
plane_b << 689.0, 1239.01, 1.0003, 0.5;
plane_ad << 24.234, -22.234, 3.0823, -24.5;
plane_bd << 689.0, 1239.01, 1.0003, 0.5;

EXPECT_TRUE (pcl::planeWithPlaneIntersection (plane_a, plane_b, line, 0.1));
EXPECT_TRUE (pcl::planeWithPlaneIntersection (plane_ad, plane_bd, lined, 0.1));

tolerance = 1e-2;
EXPECT_NEAR (line[0], 0.662983, tolerance);
EXPECT_NEAR (line[1], -0.369141, tolerance);
EXPECT_NEAR (line[2], 0.0732528, tolerance);
EXPECT_NEAR (line[3], -3841.24, tolerance);
EXPECT_NEAR (line[4], 2099.46, tolerance);
EXPECT_NEAR (line[5], 45345.4, tolerance);
EXPECT_NEAR (lined[0], 0.662983, tolerance);
EXPECT_NEAR (lined[1], -0.369141, tolerance);
EXPECT_NEAR (lined[2], 0.0732528, tolerance);
EXPECT_NEAR (lined[3], -3841.24, tolerance);
EXPECT_NEAR (lined[4], 2099.46, tolerance);
EXPECT_NEAR (lined[5], 45345.4, tolerance);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down