Point Cloud Library (PCL)  1.12.0-dev
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 
45 namespace pcl
46 {
47 
48 bool
49 lineWithLineIntersection (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 
68 bool
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 
78 template <typename Scalar> bool
79 planeWithPlaneIntersection (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 
126 template <typename Scalar> bool
127 threePlanesIntersection (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 
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
Definition: convolution.h:46
intersections.h
pcl::planeWithPlaneIntersection
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.
Definition: intersections.hpp:79
pcl::lineWithLineIntersection
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.
Definition: intersections.hpp:49
pcl::ModelCoefficients::values
std::vector< float > values
Definition: ModelCoefficients.h:19
pcl::lineToLineSegment
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.
pcl::threePlanesIntersection
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.
Definition: intersections.hpp:127
pcl::ModelCoefficients
Definition: ModelCoefficients.h:11