Point Cloud Library (PCL)  1.3.1
normal_3d.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: normal_3d.h 2631 2011-10-05 00:27:56Z mdixon $
00035  *
00036  */
00037 
00038 #ifndef PCL_NORMAL_3D_H_
00039 #define PCL_NORMAL_3D_H_
00040 
00041 #include <pcl/features/feature.h>
00042 
00043 namespace pcl
00044 {
00055   template <typename PointT> inline void 
00056   computePointNormal (const pcl::PointCloud<PointT> &cloud, 
00057                       Eigen::Vector4f &plane_parameters, float &curvature)
00058   {
00059     if (cloud.points.empty ())
00060     {
00061       plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00062       curvature = std::numeric_limits<float>::quiet_NaN ();
00063       return;
00064     }
00065     // Placeholder for the 3x3 covariance matrix at each surface patch
00066     EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00067     // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
00068     Eigen::Vector4f xyz_centroid;
00069 
00070     // Estimate the XYZ centroid
00071     compute3DCentroid (cloud, xyz_centroid);
00072 
00073     // Compute the 3x3 covariance matrix
00074     computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);
00075 
00076     // Get the plane normal and surface curvature
00077     solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
00078   }
00079 
00091   template <typename PointT> inline void 
00092   computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 
00093                       Eigen::Vector4f &plane_parameters, float &curvature)
00094   {
00095     if (indices.empty ())
00096     {
00097       plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00098       curvature = std::numeric_limits<float>::quiet_NaN ();
00099       return;
00100     }
00101     // Placeholder for the 3x3 covariance matrix at each surface patch
00102     EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00103     // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
00104     Eigen::Vector4f xyz_centroid;
00105 
00106     // Estimate the XYZ centroid
00107     compute3DCentroid (cloud, indices, xyz_centroid);
00108 
00109     // Compute the 3x3 covariance matrix
00110     computeCovarianceMatrix (cloud, indices, xyz_centroid, covariance_matrix);
00111 
00112     // Get the plane normal and surface curvature
00113     solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
00114   }
00115 
00124   template <typename PointT> inline void 
00125   flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 
00126                               Eigen::Vector4f &normal)
00127   {
00128     Eigen::Vector4f vp (vp_x, vp_y, vp_z, 0);
00129     // See if we need to flip any plane normals
00130     vp -= point.getVector4fMap ();
00131     vp[3] = 0;  // enforce the last coordinate
00132 
00133     // Dot product between the (viewpoint - point) and the plane normal
00134     float cos_theta = vp.dot (normal);
00135 
00136     // Flip the plane normal
00137     if (cos_theta < 0)
00138     {
00139       normal *= -1;
00140       normal[3] = 0;  // enforce the last coordinate;
00141       // Hessian form (D = nc . p_plane (centroid here) + p)
00142       normal[3] = -1 * normal.dot (point.getVector4fMap ());
00143     }
00144   }
00145 
00156   template <typename PointT> inline void 
00157   flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 
00158                               float &nx, float &ny, float &nz)
00159   {
00160     // See if we need to flip any plane normals
00161     vp_x -= point.x;
00162     vp_y -= point.y;
00163     vp_z -= point.z;
00164 
00165     // Dot product between the (viewpoint - point) and the plane normal
00166     float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
00167 
00168     // Flip the plane normal
00169     if (cos_theta < 0)
00170     {
00171       nx *= -1;
00172       ny *= -1;
00173       nz *= -1;
00174     }
00175   }
00176 
00185   template <typename PointInT, typename PointOutT>
00186   class NormalEstimation: public Feature<PointInT, PointOutT>
00187   {
00188     public:
00189       using Feature<PointInT, PointOutT>::feature_name_;
00190       using Feature<PointInT, PointOutT>::getClassName;
00191       using Feature<PointInT, PointOutT>::indices_;
00192       using Feature<PointInT, PointOutT>::input_;
00193       using Feature<PointInT, PointOutT>::surface_;
00194       using Feature<PointInT, PointOutT>::k_;
00195       using Feature<PointInT, PointOutT>::search_radius_;
00196       using Feature<PointInT, PointOutT>::search_parameter_;
00197 
00198       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00199 
00201       NormalEstimation () : vpx_ (0), vpy_ (0), vpz_ (0) 
00202       {
00203         feature_name_ = "NormalEstimation";
00204       };
00205 
00216       inline void 
00217       computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature)
00218       {
00219         if (indices.empty ())
00220         {
00221           plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00222           curvature = std::numeric_limits<float>::quiet_NaN ();
00223           return;
00224         }
00225         // Estimate the XYZ centroid
00226         compute3DCentroid (cloud, indices, xyz_centroid_);
00227 
00228         // Compute the 3x3 covariance matrix
00229         computeCovarianceMatrix (cloud, indices, xyz_centroid_, covariance_matrix_);
00230 
00231         // Get the plane normal and surface curvature
00232         solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature);
00233       }
00234 
00247       inline void 
00248       computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)
00249       {
00250         if (indices.empty ())
00251         {
00252           nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
00253           return;
00254         }
00255         // Estimate the XYZ centroid
00256         compute3DCentroid (cloud, indices, xyz_centroid_);
00257 
00258         // Compute the 3x3 covariance matrix
00259         computeCovarianceMatrix (cloud, indices, xyz_centroid_, covariance_matrix_);
00260 
00261         // Get the plane normal and surface curvature
00262         solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
00263       }
00264 
00270       inline void
00271       setViewPoint (float vpx, float vpy, float vpz)
00272       {
00273         vpx_ = vpx;
00274         vpy_ = vpy;
00275         vpz_ = vpz;
00276       }
00277 
00279       inline void
00280       getViewPoint (float &vpx, float &vpy, float &vpz)
00281       {
00282         vpx = vpx_;
00283         vpy = vpy_;
00284         vpz = vpz_;
00285       }
00286 
00287     protected:
00293       void computeFeature (PointCloudOut &output);
00294 
00295     private:
00298       float vpx_, vpy_, vpz_;
00299 
00301       EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_;
00302 
00304       Eigen::Vector4f xyz_centroid_;
00305   };
00306 }
00307 
00308 #endif  //#ifndef PCL_NORMAL_3D_H_
00309 
00310 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines