Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
39 
40 #if defined __GNUC__
41 # pragma GCC system_header
42 #endif
43 
44 #include <Eigen/Core>
45 #include <pcl/console/print.h>
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.getVector3fMap () - p2.getVector3fMap ();
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.getVector3fMap () - p2.getVector3fMap ();
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  /** \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.
107  *
108  * \param[in] point Point projected on the plane
109  * \param[in] plane_origin The plane origin
110  * \param[in] plane_normal The plane normal
111  * \return unit vector pointing from plane_origin to the projection of point on the plane.
112  * \ingroup geometry
113  */
114  inline Eigen::Vector3f
115  projectedAsUnitVector (Eigen::Vector3f const &point,
116  Eigen::Vector3f const &plane_origin,
117  Eigen::Vector3f const &plane_normal)
118  {
119  Eigen::Vector3f projection;
120  project (point, plane_origin, plane_normal, projection);
121  Eigen::Vector3f projected_as_unit_vector = projection - plane_origin;
122  projected_as_unit_vector.normalize ();
123  return projected_as_unit_vector;
124  }
125 
126 
127  /** \brief Define a random unit vector orthogonal to axis.
128  *
129  * \param[in] axis Axis
130  * \return random unit vector orthogonal to axis
131  * \ingroup geometry
132  */
133  inline Eigen::Vector3f
134  randomOrthogonalAxis (Eigen::Vector3f const &axis)
135  {
136  Eigen::Vector3f rand_ortho_axis;
137  rand_ortho_axis.setRandom();
138  if (std::abs (axis.z ()) > 1E-8f)
139  {
140  rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
141  }
142  else if (std::abs (axis.y ()) > 1E-8f)
143  {
144  rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
145  }
146  else if (std::abs (axis.x ()) > 1E-8f)
147  {
148  rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
149  }
150  else
151  {
152  PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f");
153  }
154 
155  rand_ortho_axis.normalize ();
156  return rand_ortho_axis;
157  }
158 
159 
160  }
161 }
A point structure representing normal coordinates and the surface curvature estimate.
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
Definition: geometry.h:134
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
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:115
A point structure representing Euclidean xyz coordinates, and the RGB color.
float squaredDistance(const PointT &p1, const PointT &p2)
Definition: geometry.h:68