Point Cloud Library (PCL)  1.9.1
geometry.h
Go to the documentation of this file.
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 #include <pcl/console/print.h>
47 
48 /**
49  * \file common/geometry.h
50  * Defines some geometrical functions and utility functions
51  * \ingroup common
52  */
53 
54 /*@{*/
55 namespace pcl
56 {
57  namespace geometry
58  {
59  /** @return the euclidean distance between 2 points */
60  template <typename PointT> inline float
61  distance (const PointT& p1, const PointT& p2)
62  {
63  Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
64  return (diff.norm ());
65  }
66 
67  /** @return the squared euclidean distance between 2 points */
68  template<typename PointT> inline float
69  squaredDistance (const PointT& p1, const PointT& p2)
70  {
71  Eigen::Vector3f diff = p1.getVector3fMap () - p2.getVector3fMap ();
72  return (diff.squaredNorm ());
73  }
74 
75  /** @return the point projection on a plane defined by its origin and normal vector
76  * \param[in] point Point to be projected
77  * \param[in] plane_origin The plane origin
78  * \param[in] plane_normal The plane normal
79  * \param[out] projected The returned projected point
80  */
81  template<typename PointT, typename NormalT> inline void
82  project (const PointT& point, const PointT &plane_origin,
83  const NormalT& plane_normal, PointT& projected)
84  {
85  Eigen::Vector3f po = point - plane_origin;
86  const Eigen::Vector3f normal = plane_normal.getVector3fMapConst ();
87  float lambda = normal.dot(po);
88  projected.getVector3fMap () = point.getVector3fMapConst () - (lambda * normal);
89  }
90 
91  /** @return the point projection on a plane defined by its origin and normal vector
92  * \param[in] point Point to be projected
93  * \param[in] plane_origin The plane origin
94  * \param[in] plane_normal The plane normal
95  * \param[out] projected The returned projected point
96  */
97  inline void
98  project (const Eigen::Vector3f& point, const Eigen::Vector3f &plane_origin,
99  const Eigen::Vector3f& plane_normal, Eigen::Vector3f& projected)
100  {
101  Eigen::Vector3f po = point - plane_origin;
102  float lambda = plane_normal.dot(po);
103  projected = point - (lambda * plane_normal);
104  }
105 
106 
107  /** \brief Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_origin to the projection of point on the plane.
108  *
109  * \param[in] point Point projected on the plane
110  * \param[in] plane_origin The plane origin
111  * \param[in] plane_normal The plane normal
112  * \return unit vector pointing from plane_origin to the projection of point on the plane.
113  * \ingroup geometry
114  */
115  inline Eigen::Vector3f
116  projectedAsUnitVector (Eigen::Vector3f const &point,
117  Eigen::Vector3f const &plane_origin,
118  Eigen::Vector3f const &plane_normal)
119  {
120  Eigen::Vector3f projection;
121  project (point, plane_origin, plane_normal, projection);
122  Eigen::Vector3f projected_as_unit_vector = projection - plane_origin;
123  projected_as_unit_vector.normalize ();
124  return projected_as_unit_vector;
125  }
126 
127 
128  /** \brief Define a random unit vector orthogonal to axis.
129  *
130  * \param[in] axis Axis
131  * \return random unit vector orthogonal to axis
132  * \ingroup geometry
133  */
134  inline Eigen::Vector3f
135  randomOrthogonalAxis (Eigen::Vector3f const &axis)
136  {
137  Eigen::Vector3f rand_ortho_axis;
138  rand_ortho_axis.setRandom();
139  if (std::abs (axis.z ()) > 1E-8f)
140  {
141  rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
142  }
143  else if (std::abs (axis.y ()) > 1E-8f)
144  {
145  rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
146  }
147  else if (std::abs (axis.x ()) > 1E-8f)
148  {
149  rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
150  }
151  else
152  {
153  PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f");
154  }
155 
156  rand_ortho_axis.normalize ();
157  return rand_ortho_axis;
158  }
159 
160 
161  }
162 }
163 
164 /*@}*/
165 #endif //#ifndef PCL_GEOMETRY_H_
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:791
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:61
pcl::geometry::randomOrthogonalAxis
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
Definition: geometry.h:135
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:619
pcl::geometry::project
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:82
pcl::geometry::squaredDistance
float squaredDistance(const PointT &p1, const PointT &p2)
Definition: geometry.h:69
pcl::geometry::projectedAsUnitVector
Eigen::Vector3f projectedAsUnitVector(Eigen::Vector3f const &point, Eigen::Vector3f const &plane_origin, Eigen::Vector3f const &plane_normal)
Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_orig...
Definition: geometry.h:116