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 1370 2011-06-19 01:06:01Z jspricke $
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.getVector4fConstMap ();
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.getVector4fConstMap ());
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>::surface_;
00193       using Feature<PointInT, PointOutT>::k_;
00194       using Feature<PointInT, PointOutT>::search_radius_;
00195       using Feature<PointInT, PointOutT>::search_parameter_;
00196 
00197       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00198 
00200       NormalEstimation () : vpx_ (0), vpy_ (0), vpz_ (0) 
00201       {
00202         feature_name_ = "NormalEstimation";
00203       };
00204 
00215       inline void 
00216       computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature)
00217       {
00218         if (indices.empty ())
00219         {
00220           plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00221           curvature = std::numeric_limits<float>::quiet_NaN ();
00222           return;
00223         }
00224         // Estimate the XYZ centroid
00225         compute3DCentroid (cloud, indices, xyz_centroid_);
00226 
00227         // Compute the 3x3 covariance matrix
00228         computeCovarianceMatrix (cloud, indices, xyz_centroid_, covariance_matrix_);
00229 
00230         // Get the plane normal and surface curvature
00231         solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature);
00232       }
00233 
00246       inline void 
00247       computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)
00248       {
00249         if (indices.empty ())
00250         {
00251           nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
00252           return;
00253         }
00254         // Estimate the XYZ centroid
00255         compute3DCentroid (cloud, indices, xyz_centroid_);
00256 
00257         // Compute the 3x3 covariance matrix
00258         computeCovarianceMatrix (cloud, indices, xyz_centroid_, covariance_matrix_);
00259 
00260         // Get the plane normal and surface curvature
00261         solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
00262       }
00263 
00269       inline void
00270       setViewPoint (float vpx, float vpy, float vpz)
00271       {
00272         vpx_ = vpx;
00273         vpy_ = vpy;
00274         vpz_ = vpz;
00275       }
00276 
00278       inline void
00279       getViewPoint (float &vpx, float &vpy, float &vpz)
00280       {
00281         vpx = vpx_;
00282         vpy = vpy_;
00283         vpz = vpz_;
00284       }
00285 
00286     protected:
00292       void computeFeature (PointCloudOut &output);
00293 
00294     private:
00297       float vpx_, vpy_, vpz_;
00298 
00300       EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_;
00301 
00303       Eigen::Vector4f xyz_centroid_;
00304   };
00305 }
00306 
00307 #endif  //#ifndef PCL_NORMAL_3D_H_
00308 
00309