Point Cloud Library (PCL) 1.13.0
intersections.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38#pragma once
39
41#include <pcl/pcl_macros.h>
42#include <pcl/console/print.h>
43
44
45namespace pcl
46{
47
48bool
49lineWithLineIntersection (const Eigen::VectorXf &line_a,
50 const Eigen::VectorXf &line_b,
51 Eigen::Vector4f &point, double sqr_eps)
52{
53 Eigen::Vector4f p1, p2;
54 lineToLineSegment (line_a, line_b, p1, p2);
55
56 // If the segment size is smaller than a pre-given epsilon...
57 double sqr_dist = (p1 - p2).squaredNorm ();
58 if (sqr_dist < sqr_eps)
59 {
60 point = p1;
61 return (true);
62 }
63 point.setZero ();
64 return (false);
65}
66
67
68bool
70 const pcl::ModelCoefficients &line_b,
71 Eigen::Vector4f &point, double sqr_eps)
72{
73 Eigen::VectorXf coeff1 = Eigen::VectorXf::Map (&line_a.values[0], line_a.values.size ());
74 Eigen::VectorXf coeff2 = Eigen::VectorXf::Map (&line_b.values[0], line_b.values.size ());
75 return (lineWithLineIntersection (coeff1, coeff2, point, sqr_eps));
76}
77
78template <typename Scalar> bool
79planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
80 const Eigen::Matrix<Scalar, 4, 1> &plane_b,
81 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
82 double angular_tolerance)
83{
84 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
85 using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
86 using Vector5 = Eigen::Matrix<Scalar, 5, 1>;
87 using Matrix5 = Eigen::Matrix<Scalar, 5, 5>;
88
89 // Normalize plane normals
90 Vector3 plane_a_norm (plane_a.template head<3> ());
91 Vector3 plane_b_norm (plane_b.template head<3> ());
92 plane_a_norm.normalize ();
93 plane_b_norm.normalize ();
94
95 // Test if planes are parallel
96 double test_cos = plane_a_norm.dot (plane_b_norm);
97 double tolerance_cos = 1 - sin (std::abs (angular_tolerance));
98
99 if (std::abs (test_cos) > tolerance_cos)
100 {
101 PCL_DEBUG ("Plane A and Plane B are parallel.\n");
102 return (false);
103 }
104
105 Vector4 line_direction = plane_a.cross3 (plane_b);
106 line_direction.normalized();
107
108 // Construct system of equations using lagrange multipliers with one objective function and two constraints
109 Matrix5 langrange_coefs;
110 langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
111 0,2,0, plane_a[1], plane_b[1],
112 0,0,2, plane_a[2], plane_b[2],
113 plane_a[0], plane_a[1], plane_a[2], 0, 0,
114 plane_b[0], plane_b[1], plane_b[2], 0, 0;
115
116 Vector5 b;
117 b << 0, 0, 0, -plane_a[3], -plane_b[3];
118
119 line.resize(6);
120 // Solve for the lagrange multipliers
121 line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
122 line.template tail<3>() = line_direction.template head<3>();
123 return (true);
124}
125
126template <typename Scalar> bool
127threePlanesIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
128 const Eigen::Matrix<Scalar, 4, 1> &plane_b,
129 const Eigen::Matrix<Scalar, 4, 1> &plane_c,
130 Eigen::Matrix<Scalar, 3, 1> &intersection_point,
131 double determinant_tolerance)
132{
133 using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
134 using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
135
136 // TODO: Using Eigen::HyperPlanes is better to solve this problem
137 // Check if some planes are parallel
138 Matrix3 normals_in_lines;
139
140 for (int i = 0; i < 3; i++)
141 {
142 normals_in_lines (i, 0) = plane_a[i];
143 normals_in_lines (i, 1) = plane_b[i];
144 normals_in_lines (i, 2) = plane_c[i];
145 }
146
147 Scalar determinant = normals_in_lines.determinant ();
148 if (std::abs (determinant) < determinant_tolerance)
149 {
150 // det ~= 0
151 PCL_DEBUG ("At least two planes are parallel.\n");
152 return (false);
153 }
154
155 // Left part of the 3 equations
156 Matrix3 left_member;
157
158 for (int i = 0; i < 3; i++)
159 {
160 left_member (0, i) = plane_a[i];
161 left_member (1, i) = plane_b[i];
162 left_member (2, i) = plane_c[i];
163 }
164
165 // Right side of the 3 equations
166 Vector3 right_member;
167 right_member << -plane_a[3], -plane_b[3], -plane_c[3];
168
169 // Solve the system
170 intersection_point = left_member.fullPivLu ().solve (right_member);
171 return (true);
172}
173
174} // namespace pcl
175
bool lineWithLineIntersection(const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &point, double sqr_eps)
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.
Define line with line intersection functions.
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)
Determine the point of intersection of three non-parallel planes by solving the equations.
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)
Determine the line of intersection of two non-parallel planes using lagrange multipliers.
Defines all the PCL and non-PCL macros used.
std::vector< float > values