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) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id: cvfh.hpp 5311 2012-03-26 22:02:04Z aaldoma $ 00038 * 00039 */ 00040 00041 #ifndef PCL_FEATURES_IMPL_CRH_H_ 00042 #define PCL_FEATURES_IMPL_CRH_H_ 00043 00044 #include <pcl/features/crh.h> 00045 #include <pcl/common/fft/kiss_fftr.h> 00046 #include <pcl/common/common.h> 00047 #include <pcl/common/transforms.h> 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////// 00050 template<typename PointInT, typename PointNT, typename PointOutT> 00051 void 00052 pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00053 { 00054 // Check if input was set 00055 if (!normals_) 00056 { 00057 PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ()); 00058 output.width = output.height = 0; 00059 output.points.clear (); 00060 return; 00061 } 00062 00063 if (normals_->points.size () != surface_->points.size ()) 00064 { 00065 PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ()); 00066 output.width = output.height = 0; 00067 output.points.clear (); 00068 return; 00069 } 00070 00071 Eigen::Vector3f plane_normal; 00072 plane_normal[0] = -centroid_[0]; 00073 plane_normal[1] = -centroid_[1]; 00074 plane_normal[2] = -centroid_[2]; 00075 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ (); 00076 plane_normal.normalize (); 00077 Eigen::Vector3f axis = plane_normal.cross (z_vector); 00078 double rotation = -asin (axis.norm ()); 00079 axis.normalize (); 00080 00081 int nbins = nbins_; 00082 int bin_angle = 360 / nbins; 00083 00084 Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis)); 00085 00086 pcl::PointCloud<pcl::PointNormal> grid; 00087 grid.points.resize (indices_->size ()); 00088 00089 for (size_t i = 0; i < indices_->size (); i++) 00090 { 00091 grid.points[i].getVector4fMap () = surface_->points[(*indices_)[i]].getVector4fMap (); 00092 grid.points[i].getNormalVector4fMap () = normals_->points[(*indices_)[i]].getNormalVector4fMap (); 00093 } 00094 00095 pcl::transformPointCloudWithNormals (grid, grid, transformPC); 00096 00097 //fill spatial data vector 00098 kiss_fft_scalar * spatial_data = new kiss_fft_scalar[nbins]; 00099 float sum_w = 0, w = 0; 00100 int bin = 0; 00101 for (size_t i = 0; i < grid.points.size (); ++i) 00102 { 00103 bin = static_cast<int> ((((atan2 (grid.points[i].normal_y, grid.points[i].normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins; 00104 w = sqrtf (grid.points[i].normal_y * grid.points[i].normal_y + grid.points[i].normal_x * grid.points[i].normal_x); 00105 sum_w += w; 00106 spatial_data[bin] += w; 00107 } 00108 00109 for (int i = 0; i < nbins; ++i) 00110 spatial_data[i] /= sum_w; 00111 00112 kiss_fft_cpx * freq_data = new kiss_fft_cpx[nbins / 2 + 1]; 00113 kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, NULL, NULL); 00114 kiss_fftr (mycfg, spatial_data, freq_data); 00115 00116 output.points.resize (1); 00117 output.width = output.height = 1; 00118 00119 output.points[0].histogram[0] = freq_data[0].r / freq_data[0].r; //dc 00120 int k = 1; 00121 for (int i = 1; i < (nbins / 2); i++, k += 2) 00122 { 00123 output.points[0].histogram[k] = freq_data[i].r / freq_data[0].r; 00124 output.points[0].histogram[k + 1] = freq_data[i].i / freq_data[0].r; 00125 } 00126 00127 output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r / freq_data[0].r; //nyquist 00128 00129 delete[] spatial_data; 00130 delete[] freq_data; 00131 00132 } 00133 00134 #define PCL_INSTANTIATE_CRHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CRHEstimation<T,NT,OutT>; 00135 00136 #endif // PCL_FEATURES_IMPL_CRH_H_