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 tolerance_cos = 1 - sin (fabs (angular_tolerance));
96 if (fabs (test_cos) > tolerance_cos)
98 PCL_DEBUG (
"Plane A and Plane B are parallel.\n");
102 Vector4 line_direction = plane_a.cross3 (plane_b);
103 line_direction.normalized();
106 Matrix5 langrange_coefs;
107 langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
108 0,2,0, plane_a[1], plane_b[1],
109 0,0,2, plane_a[2], plane_b[2],
110 plane_a[0], plane_a[1], plane_a[2], 0, 0,
111 plane_b[0], plane_b[1], plane_b[2], 0, 0;
114 b << 0, 0, 0, -plane_a[3], -plane_b[3];
118 line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
119 line.template tail<3>() = line_direction.template head<3>();
123 template <
typename Scalar>
bool 125 const Eigen::Matrix<Scalar, 4, 1> &plane_b,
126 const Eigen::Matrix<Scalar, 4, 1> &plane_c,
127 Eigen::Matrix<Scalar, 3, 1> &intersection_point,
128 double determinant_tolerance)
130 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
131 typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
135 Matrix3 normals_in_lines;
137 for (
int i = 0; i < 3; i++)
139 normals_in_lines (i, 0) = plane_a[i];
140 normals_in_lines (i, 1) = plane_b[i];
141 normals_in_lines (i, 2) = plane_c[i];
144 Scalar determinant = normals_in_lines.determinant ();
145 if (fabs (determinant) < determinant_tolerance)
148 PCL_DEBUG (
"At least two planes are parralel.\n");
155 for (
int i = 0; i < 3; i++)
157 left_member (0, i) = plane_a[i];
158 left_member (1, i) = plane_b[i];
159 left_member (2, i) = plane_c[i];
163 Vector3 right_member;
164 right_member << -plane_a[3], -plane_b[3], -plane_c[3];
167 intersection_point = left_member.fullPivLu ().solve (right_member);
171 #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.