Point Cloud Library (PCL)  1.8.1
intersections.h
Go to the documentation of this file.
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 #ifndef PCL_INTERSECTIONS_H_
39 #define PCL_INTERSECTIONS_H_
40 
41 #include <pcl/ModelCoefficients.h>
42 #include <pcl/common/common.h>
43 #include <pcl/common/distances.h>
44 
45 /**
46  * \file pcl/common/intersections.h
47  * Define line with line intersection functions
48  * \ingroup common
49  */
50 
51 /*@{*/
52 namespace pcl
53 {
54  /** \brief Get the intersection of a two 3D lines in space as a 3D point
55  * \param[in] line_a the coefficients of the first line (point, direction)
56  * \param[in] line_b the coefficients of the second line (point, direction)
57  * \param[out] point holder for the computed 3D point
58  * \param[in] sqr_eps maximum allowable squared distance to the true solution
59  * \ingroup common
60  */
61  PCL_EXPORTS inline bool
62  lineWithLineIntersection (const Eigen::VectorXf &line_a,
63  const Eigen::VectorXf &line_b,
64  Eigen::Vector4f &point,
65  double sqr_eps = 1e-4);
66 
67  /** \brief Get the intersection of a two 3D lines in space as a 3D point
68  * \param[in] line_a the coefficients of the first line (point, direction)
69  * \param[in] line_b the coefficients of the second line (point, direction)
70  * \param[out] point holder for the computed 3D point
71  * \param[in] sqr_eps maximum allowable squared distance to the true solution
72  * \ingroup common
73  */
74 
75  PCL_EXPORTS inline bool
77  const pcl::ModelCoefficients &line_b,
78  Eigen::Vector4f &point,
79  double sqr_eps = 1e-4);
80 
81  /** \brief Determine the line of intersection of two non-parallel planes using lagrange multipliers
82  * \note Described in: "Intersection of Two Planes, John Krumm, Microsoft Research, Redmond, WA, USA"
83  * \param[in] plane_a coefficients of plane A and plane B in the form ax + by + cz + d = 0
84  * \param[in] plane_b coefficients of line where line.tail<3>() = direction vector and
85  * line.head<3>() the point on the line clossest to (0, 0, 0)
86  * \param[out] line the intersected line to be filled
87  * \param[in] angular_tolerance tolerance in radians
88  * \return true if succeeded/planes aren't parallel
89  */
90  PCL_EXPORTS template <typename Scalar> bool
91  planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
92  const Eigen::Matrix<Scalar, 4, 1> &plane_b,
93  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
94  double angular_tolerance = 0.1);
95 
96  PCL_EXPORTS inline bool
97  planeWithPlaneIntersection (const Eigen::Vector4f &plane_a,
98  const Eigen::Vector4f &plane_b,
99  Eigen::VectorXf &line,
100  double angular_tolerance = 0.1)
101  {
102  return (planeWithPlaneIntersection<float> (plane_a, plane_b, line, angular_tolerance));
103  }
104 
105  PCL_EXPORTS inline bool
106  planeWithPlaneIntersection (const Eigen::Vector4d &plane_a,
107  const Eigen::Vector4d &plane_b,
108  Eigen::VectorXd &line,
109  double angular_tolerance = 0.1)
110  {
111  return (planeWithPlaneIntersection<double> (plane_a, plane_b, line, angular_tolerance));
112  }
113 
114  /** \brief Determine the point of intersection of three non-parallel planes by solving the equations.
115  * \note If using nearly parralel planes you can lower the determinant_tolerance value. This can
116  * lead to inconsistent results.
117  * If the three planes intersects in a line the point will be anywhere on the line.
118  * \param[in] plane_a are the coefficients of the first plane in the form ax + by + cz + d = 0
119  * \param[in] plane_b are the coefficients of the second plane
120  * \param[in] plane_c are the coefficients of the third plane
121  * \param[in] determinant_tolerance is a limit to determine whether planes are parallel or not
122  * \param[out] intersection_point the three coordinates x, y, z of the intersection point
123  * \return true if succeeded/planes aren't parallel
124  */
125  PCL_EXPORTS template <typename Scalar> bool
126  threePlanesIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
127  const Eigen::Matrix<Scalar, 4, 1> &plane_b,
128  const Eigen::Matrix<Scalar, 4, 1> &plane_c,
129  Eigen::Matrix<Scalar, 3, 1> &intersection_point,
130  double determinant_tolerance = 1e-6);
131 
132 
133  PCL_EXPORTS inline bool
134  threePlanesIntersection (const Eigen::Vector4f &plane_a,
135  const Eigen::Vector4f &plane_b,
136  const Eigen::Vector4f &plane_c,
137  Eigen::Vector3f &intersection_point,
138  double determinant_tolerance = 1e-6)
139  {
140  return (threePlanesIntersection<float> (plane_a, plane_b, plane_c,
141  intersection_point, determinant_tolerance));
142  }
143 
144  PCL_EXPORTS inline bool
145  threePlanesIntersection (const Eigen::Vector4d &plane_a,
146  const Eigen::Vector4d &plane_b,
147  const Eigen::Vector4d &plane_c,
148  Eigen::Vector3d &intersection_point,
149  double determinant_tolerance = 1e-6)
150  {
151  return (threePlanesIntersection<double> (plane_a, plane_b, plane_c,
152  intersection_point, determinant_tolerance));
153  }
154 
155 }
156 /*@}*/
157 
158 #include <pcl/common/impl/intersections.hpp>
159 
160 #endif //#ifndef PCL_INTERSECTIONS_H_
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.
Define standard C methods to do distance calculations.
Define standard C methods and C++ classes that are common to all methods.
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.