template <typename Scalar> bool
pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
const Eigen::Matrix<Scalar, 4, 1> &plane_b,
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, 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 ();
// Test if planes are parallel
double test_cos = plane_a_norm.dot (plane_b_norm);
double tolerance_cos = 1 - sin (fabs (angular_tolerance));
if (fabs (test_cos) > tolerance_cos)
{
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
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];
line.resize(6);
// Solve for the lagrange multipliers
line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
line.template tail<3>() = line_direction.template head<3>();
return (true);
}
