Point Cloud Library (PCL) 1.13.0
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#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/*@{*/
54namespace 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\n");
153 }
154
155 rand_ortho_axis.normalize ();
156 return rand_ortho_axis;
157 }
158
159
160 }
161}
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
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
Definition: geometry.h:134
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
float squaredDistance(const PointT &p1, const PointT &p2)
Definition: geometry.h:68
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates, and the RGB color.