00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef PCL_NORMAL_3D_H_
00041 #define PCL_NORMAL_3D_H_
00042
00043 #include <pcl/features/feature.h>
00044
00045 namespace pcl
00046 {
00057 template <typename PointT> inline void
00058 computePointNormal (const pcl::PointCloud<PointT> &cloud,
00059 Eigen::Vector4f &plane_parameters, float &curvature)
00060 {
00061
00062 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00063
00064 Eigen::Vector4f xyz_centroid;
00065
00066 if (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0)
00067 {
00068 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00069 curvature = std::numeric_limits<float>::quiet_NaN ();
00070 return;
00071 }
00072
00073
00074 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
00075 }
00076
00088 template <typename PointT> inline void
00089 computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00090 Eigen::Vector4f &plane_parameters, float &curvature)
00091 {
00092
00093 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
00094
00095 Eigen::Vector4f xyz_centroid;
00096 if (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0)
00097 {
00098 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00099 curvature = std::numeric_limits<float>::quiet_NaN ();
00100 return;
00101 }
00102
00103 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
00104 }
00105
00114 template <typename PointT, typename Scalar> inline void
00115 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
00116 Eigen::Matrix<Scalar, 4, 1>& normal)
00117 {
00118 Eigen::Matrix <Scalar, 4, 1> vp (vp_x - point.x, vp_y - point.x, vp_z - point.z, 0);
00119
00120
00121 float cos_theta = vp.dot (normal);
00122
00123
00124 if (cos_theta < 0)
00125 {
00126 normal *= -1;
00127 normal[3] = 0.0f;
00128
00129 normal[3] = -1 * normal.dot (point.getVector4fMap ());
00130 }
00131 }
00132
00141 template <typename PointT, typename Scalar> inline void
00142 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
00143 Eigen::Matrix<Scalar, 3, 1>& normal)
00144 {
00145 Eigen::Matrix <Scalar, 3, 1> vp (vp_x - point.x, vp_y - point.x, vp_z - point.z);
00146
00147
00148 if (vp.dot (normal) < 0)
00149 normal *= -1;
00150 }
00151
00162 template <typename PointT> inline void
00163 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
00164 float &nx, float &ny, float &nz)
00165 {
00166
00167 vp_x -= point.x;
00168 vp_y -= point.y;
00169 vp_z -= point.z;
00170
00171
00172 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
00173
00174
00175 if (cos_theta < 0)
00176 {
00177 nx *= -1;
00178 ny *= -1;
00179 nz *= -1;
00180 }
00181 }
00182
00192 template <typename PointInT, typename PointOutT>
00193 class NormalEstimation: public Feature<PointInT, PointOutT>
00194 {
00195 public:
00196 using Feature<PointInT, PointOutT>::feature_name_;
00197 using Feature<PointInT, PointOutT>::getClassName;
00198 using Feature<PointInT, PointOutT>::indices_;
00199 using Feature<PointInT, PointOutT>::input_;
00200 using Feature<PointInT, PointOutT>::surface_;
00201 using Feature<PointInT, PointOutT>::k_;
00202 using Feature<PointInT, PointOutT>::search_radius_;
00203 using Feature<PointInT, PointOutT>::search_parameter_;
00204
00205 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00206 typedef typename Feature<PointInT, PointOutT>::PointCloudConstPtr PointCloudConstPtr;
00207
00209 NormalEstimation ()
00210 : vpx_ (0)
00211 , vpy_ (0)
00212 , vpz_ (0)
00213 , covariance_matrix_ ()
00214 , xyz_centroid_ ()
00215 , use_sensor_origin_ (true)
00216 {
00217 feature_name_ = "NormalEstimation";
00218 };
00219
00230 inline void
00231 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature)
00232 {
00233 if (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
00234 {
00235 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
00236 curvature = std::numeric_limits<float>::quiet_NaN ();
00237 return;
00238 }
00239
00240
00241 solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature);
00242 }
00243
00256 inline void
00257 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature)
00258 {
00259 if (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
00260 {
00261 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
00262 return;
00263 }
00264
00265
00266 solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
00267 }
00268
00272 virtual inline void
00273 setInputCloud (const PointCloudConstPtr &cloud)
00274 {
00275 input_ = cloud;
00276 if (use_sensor_origin_)
00277 {
00278 vpx_ = input_->sensor_origin_.coeff (0);
00279 vpy_ = input_->sensor_origin_.coeff (1);
00280 vpz_ = input_->sensor_origin_.coeff (2);
00281 }
00282 }
00283
00289 inline void
00290 setViewPoint (float vpx, float vpy, float vpz)
00291 {
00292 vpx_ = vpx;
00293 vpy_ = vpy;
00294 vpz_ = vpz;
00295 use_sensor_origin_ = false;
00296 }
00297
00306 inline void
00307 getViewPoint (float &vpx, float &vpy, float &vpz)
00308 {
00309 vpx = vpx_;
00310 vpy = vpy_;
00311 vpz = vpz_;
00312 }
00313
00318 inline void
00319 useSensorOriginAsViewPoint ()
00320 {
00321 use_sensor_origin_ = true;
00322 if (input_)
00323 {
00324 vpx_ = input_->sensor_origin_.coeff (0);
00325 vpy_ = input_->sensor_origin_.coeff (1);
00326 vpz_ = input_->sensor_origin_.coeff (2);
00327 }
00328 else
00329 {
00330 vpx_ = 0;
00331 vpy_ = 0;
00332 vpz_ = 0;
00333 }
00334 }
00335
00336 protected:
00342 void
00343 computeFeature (PointCloudOut &output);
00344
00347 float vpx_, vpy_, vpz_;
00348
00350 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_;
00351
00353 Eigen::Vector4f xyz_centroid_;
00354
00356 bool use_sensor_origin_;
00357
00358 private:
00362 void
00363 computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &) {}
00364 };
00365
00374 template <typename PointInT>
00375 class NormalEstimation<PointInT, Eigen::MatrixXf>: public NormalEstimation<PointInT, pcl::Normal>
00376 {
00377 public:
00378 using NormalEstimation<PointInT, pcl::Normal>::indices_;
00379 using NormalEstimation<PointInT, pcl::Normal>::input_;
00380 using NormalEstimation<PointInT, pcl::Normal>::surface_;
00381 using NormalEstimation<PointInT, pcl::Normal>::k_;
00382 using NormalEstimation<PointInT, pcl::Normal>::search_parameter_;
00383 using NormalEstimation<PointInT, pcl::Normal>::vpx_;
00384 using NormalEstimation<PointInT, pcl::Normal>::vpy_;
00385 using NormalEstimation<PointInT, pcl::Normal>::vpz_;
00386 using NormalEstimation<PointInT, pcl::Normal>::computePointNormal;
00387 using NormalEstimation<PointInT, pcl::Normal>::compute;
00388
00389 private:
00395 void
00396 computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output);
00397
00401 void
00402 compute (pcl::PointCloud<pcl::Normal> &) {}
00403 };
00404 }
00405
00406 #endif //#ifndef PCL_NORMAL_3D_H_
00407
00408