38 #ifndef PCL_COMMON_INTERSECTIONS_IMPL_HPP_
39 #define PCL_COMMON_INTERSECTIONS_IMPL_HPP_
41 #include <pcl/pcl_macros.h>
42 #include <pcl/console/print.h>
48 const Eigen::VectorXf &line_b,
49 Eigen::Vector4f &point,
double sqr_eps)
51 Eigen::Vector4f p1, p2;
55 double sqr_dist = (p1 - p2).squaredNorm ();
56 if (sqr_dist < sqr_eps)
68 Eigen::Vector4f &point,
double sqr_eps)
70 Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.
values[0], line_a.
values.size ());
71 Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.
values[0], line_b.
values.size ());
75 template <
typename Scalar>
bool
77 const Eigen::Matrix<Scalar, 4, 1> &plane_b,
78 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
79 double angular_tolerance)
81 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
82 typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
83 typedef Eigen::Matrix<Scalar, 5, 1> Vector5;
84 typedef Eigen::Matrix<Scalar, 5, 5> Matrix5;
87 Vector3 plane_a_norm (plane_a.template head<3> ());
88 Vector3 plane_b_norm (plane_b.template head<3> ());
89 plane_a_norm.normalize ();
90 plane_b_norm.normalize ();
93 double test_cos = plane_a_norm.dot (plane_b_norm);
94 double upper_limit = 1 + angular_tolerance;
95 double lower_limit = 1 - angular_tolerance;
97 if ((test_cos > lower_limit) && (test_cos < upper_limit))
99 PCL_DEBUG (
"Plane A and Plane B are parallel.\n");
103 Vector4 line_direction = plane_a.cross3 (plane_b);
104 line_direction.normalized();
107 Matrix5 langrange_coefs;
108 langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
109 0,2,0, plane_a[1], plane_b[1],
110 0,0,2, plane_a[2], plane_b[2],
111 plane_a[0], plane_a[1], plane_a[2], 0, 0,
112 plane_b[0], plane_b[1], plane_b[2], 0, 0;
115 b << 0, 0, 0, -plane_a[3], -plane_b[3];
119 line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
120 line.template tail<3>() = line_direction.template head<3>();
124 template <
typename Scalar>
bool
126 const Eigen::Matrix<Scalar, 4, 1> &plane_b,
127 const Eigen::Matrix<Scalar, 4, 1> &plane_c,
128 Eigen::Matrix<Scalar, 3, 1> &intersection_point,
129 double determinant_tolerance)
131 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
132 typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
136 Matrix3 normals_in_lines;
138 for (
int i = 0; i < 3; i++)
140 normals_in_lines (i, 0) = plane_a[i];
141 normals_in_lines (i, 1) = plane_b[i];
142 normals_in_lines (i, 2) = plane_c[i];
145 Scalar determinant = normals_in_lines.determinant ();
146 if (fabs (determinant) < determinant_tolerance)
149 PCL_DEBUG (
"At least two planes are parralel.\n");
156 for (
int i = 0; i < 3; i++)
158 left_member (0, i) = plane_a[i];
159 left_member (1, i) = plane_b[i];
160 left_member (2, i) = plane_c[i];
164 Vector3 right_member;
165 right_member << -plane_a[3], -plane_b[3], -plane_c[3];
168 intersection_point = left_member.fullPivLu ().solve (right_member);
172 #endif //PCL_COMMON_INTERSECTIONS_IMPL_HPP
PCL_EXPORTS bool threePlanesIntersection(const Eigen::Matrix< Scalar, 4, 1 > &plane_a, const Eigen::Matrix< Scalar, 4, 1 > &plane_b, const Eigen::Matrix< Scalar, 4, 1 > &plane_c, Eigen::Matrix< Scalar, 3, 1 > &intersection_point, double determinant_tolerance=1e-6)
Determine the point of intersection of three non-parallel planes by solving the equations.
PCL_EXPORTS bool 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=0.1)
Determine the line of intersection of two non-parallel planes using lagrange multipliers.
std::vector< float > values
PCL_EXPORTS bool lineWithLineIntersection(const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &point, double sqr_eps=1e-4)
Get the intersection of a two 3D lines in space as a 3D point.
PCL_EXPORTS void lineToLineSegment(const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg)
Get the shortest 3D segment between two 3D lines.