Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_GEOMETRY_H_ 00039 #define PCL_GEOMETRY_H_ 00040 00041 #if defined __GNUC__ 00042 # pragma GCC system_header 00043 #endif 00044 00045 #include <Eigen/Core> 00046 00047 /** 00048 * \file common/geometry.h 00049 * Defines some geometrical functions and utility functions 00050 * \ingroup common 00051 */ 00052 00053 /*@{*/ 00054 namespace pcl 00055 { 00056 namespace geometry 00057 { 00058 /** @return the euclidean distance between 2 points */ 00059 template <typename PointT> inline float 00060 distance (const PointT& p1, const PointT& p2) 00061 { 00062 Eigen::Vector3f diff = p1 -p2; 00063 return (diff.norm ()); 00064 } 00065 00066 /** @return the squared euclidean distance between 2 points */ 00067 template<typename PointT> inline float 00068 squaredDistance (const PointT& p1, const PointT& p2) 00069 { 00070 Eigen::Vector3f diff = p1 -p2; 00071 return (diff.squaredNorm ()); 00072 } 00073 00074 /** @return the point projection on a plane defined by its origin and normal vector 00075 * \param[in] point Point to be projected 00076 * \param[in] plane_origin The plane origin 00077 * \param[in] plane_normal The plane normal 00078 * \param[out] projected The returned projected point 00079 */ 00080 template<typename PointT, typename NormalT> inline void 00081 project (const PointT& point, const PointT &plane_origin, 00082 const NormalT& plane_normal, PointT& projected) 00083 { 00084 Eigen::Vector3f po = point - plane_origin; 00085 const Eigen::Vector3f normal = plane_normal.getVector3fMapConst (); 00086 float lambda = normal.dot(po); 00087 projected.getVector3fMap () = point.getVector3fMapConst () - (lambda * normal); 00088 } 00089 00090 /** @return the point projection on a plane defined by its origin and normal vector 00091 * \param[in] point Point to be projected 00092 * \param[in] plane_origin The plane origin 00093 * \param[in] plane_normal The plane normal 00094 * \param[out] projected The returned projected point 00095 */ 00096 inline void 00097 project (const Eigen::Vector3f& point, const Eigen::Vector3f &plane_origin, 00098 const Eigen::Vector3f& plane_normal, Eigen::Vector3f& projected) 00099 { 00100 Eigen::Vector3f po = point - plane_origin; 00101 float lambda = plane_normal.dot(po); 00102 projected = point - (lambda * plane_normal); 00103 } 00104 } 00105 } 00106 00107 /*@}*/ 00108 #endif //#ifndef PCL_GEOMETRY_H_