Point Cloud Library (PCL)  1.7.0
geometry.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_GEOMETRY_H_
39 #define PCL_GEOMETRY_H_
40 
41 #if defined __GNUC__
42 # pragma GCC system_header
43 #endif
44 
45 #include <Eigen/Core>
46 
47 /**
48  * \file common/geometry.h
49  * Defines some geometrical functions and utility functions
50  * \ingroup common
51  */
52 
53 /*@{*/
54 namespace pcl
55 {
56  namespace geometry
57  {
58  /** @return the euclidean distance between 2 points */
59  template <typename PointT> inline float
60  distance (const PointT& p1, const PointT& p2)
61  {
62  Eigen::Vector3f diff = p1 -p2;
63  return (diff.norm ());
64  }
65 
66  /** @return the squared euclidean distance between 2 points */
67  template<typename PointT> inline float
68  squaredDistance (const PointT& p1, const PointT& p2)
69  {
70  Eigen::Vector3f diff = p1 -p2;
71  return (diff.squaredNorm ());
72  }
73 
74  /** @return the point projection on a plane defined by its origin and normal vector
75  * \param[in] point Point to be projected
76  * \param[in] plane_origin The plane origin
77  * \param[in] plane_normal The plane normal
78  * \param[out] projected The returned projected point
79  */
80  template<typename PointT, typename NormalT> inline void
81  project (const PointT& point, const PointT &plane_origin,
82  const NormalT& plane_normal, PointT& projected)
83  {
84  Eigen::Vector3f po = point - plane_origin;
85  const Eigen::Vector3f normal = plane_normal.getVector3fMapConst ();
86  float lambda = normal.dot(po);
87  projected.getVector3fMap () = point.getVector3fMapConst () - (lambda * normal);
88  }
89 
90  /** @return the point projection on a plane defined by its origin and normal vector
91  * \param[in] point Point to be projected
92  * \param[in] plane_origin The plane origin
93  * \param[in] plane_normal The plane normal
94  * \param[out] projected The returned projected point
95  */
96  inline void
97  project (const Eigen::Vector3f& point, const Eigen::Vector3f &plane_origin,
98  const Eigen::Vector3f& plane_normal, Eigen::Vector3f& projected)
99  {
100  Eigen::Vector3f po = point - plane_origin;
101  float lambda = plane_normal.dot(po);
102  projected = point - (lambda * plane_normal);
103  }
104  }
105 }
106 
107 /*@}*/
108 #endif //#ifndef PCL_GEOMETRY_H_