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_FEATURES_IMPL_PPFRGB_H_ 00039 #define PCL_FEATURES_IMPL_PPFRGB_H_ 00040 00041 #include <pcl/features/ppfrgb.h> 00042 #include <pcl/features/pfhrgb.h> 00043 00044 ////////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointInT, typename PointNT, typename PointOutT> 00046 pcl::PPFRGBEstimation<PointInT, PointNT, PointOutT>::PPFRGBEstimation () 00047 : FeatureFromNormals <PointInT, PointNT, PointOutT> () 00048 { 00049 feature_name_ = "PPFRGBEstimation"; 00050 // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () 00051 Feature<PointInT, PointOutT>::tree_.reset (new pcl::search::KdTree <PointInT> ()); 00052 Feature<PointInT, PointOutT>::search_radius_ = 1.0f; 00053 } 00054 00055 00056 ////////////////////////////////////////////////////////////////////////////////////////////// 00057 template <typename PointInT, typename PointNT, typename PointOutT> void 00058 pcl::PPFRGBEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00059 { 00060 // Initialize output container - overwrite the sizes done by Feature::initCompute () 00061 output.points.resize (indices_->size () * input_->points.size ()); 00062 output.height = 1; 00063 output.width = static_cast<uint32_t> (output.points.size ()); 00064 00065 // Compute point pair features for every pair of points in the cloud 00066 for (size_t index_i = 0; index_i < indices_->size (); ++index_i) 00067 { 00068 size_t i = (*indices_)[index_i]; 00069 for (size_t j = 0 ; j < input_->points.size (); ++j) 00070 { 00071 PointOutT p; 00072 if (i != j) 00073 { 00074 if (pcl::computeRGBPairFeatures 00075 (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), 00076 input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), 00077 p.f1, p.f2, p.f3, p.f4, p.r_ratio, p.g_ratio, p.b_ratio)) 00078 { 00079 // Calculate alpha_m angle 00080 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), 00081 model_reference_normal = normals_->points[i].getNormalVector3fMap (), 00082 model_point = input_->points[j].getVector3fMap (); 00083 Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), 00084 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); 00085 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; 00086 00087 Eigen::Vector3f model_point_transformed = transform_mg * model_point; 00088 float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); 00089 if (sin (angle) * model_point_transformed(2) < 0.0f) 00090 angle *= (-1); 00091 p.alpha_m = -angle; 00092 } 00093 else 00094 { 00095 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); 00096 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; 00097 } 00098 } 00099 // Do not calculate the feature for identity pairs (i, i) as they are not used 00100 // in the following computations 00101 else 00102 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f; 00103 00104 output.points[index_i*input_->points.size () + j] = p; 00105 } 00106 } 00107 } 00108 00109 00110 00111 ////////////////////////////////////////////////////////////////////////////////////////////// 00112 ////////////////////////////////////////////////////////////////////////////////////////////// 00113 template <typename PointInT, typename PointNT, typename PointOutT> 00114 pcl::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::PPFRGBRegionEstimation () 00115 : FeatureFromNormals <PointInT, PointNT, PointOutT> () 00116 { 00117 feature_name_ = "PPFRGBEstimation"; 00118 } 00119 00120 ////////////////////////////////////////////////////////////////////////////////////////////// 00121 template <typename PointInT, typename PointNT, typename PointOutT> void 00122 pcl::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00123 { 00124 PCL_INFO ("before computing output size: %u\n", output.size ()); 00125 output.resize (indices_->size ()); 00126 for (int index_i = 0; index_i < static_cast<int> (indices_->size ()); ++index_i) 00127 { 00128 int i = (*indices_)[index_i]; 00129 std::vector<int> nn_indices; 00130 std::vector<float> nn_distances; 00131 tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances); 00132 00133 PointOutT average_feature_nn; 00134 average_feature_nn.alpha_m = 0; 00135 average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 = 00136 average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f; 00137 00138 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it) 00139 { 00140 int j = *nn_it; 00141 if (i != j) 00142 { 00143 float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio; 00144 if (pcl::computeRGBPairFeatures 00145 (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (), 00146 input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (), 00147 f1, f2, f3, f4, r_ratio, g_ratio, b_ratio)) 00148 { 00149 average_feature_nn.f1 += f1; 00150 average_feature_nn.f2 += f2; 00151 average_feature_nn.f3 += f3; 00152 average_feature_nn.f4 += f4; 00153 average_feature_nn.r_ratio += r_ratio; 00154 average_feature_nn.g_ratio += g_ratio; 00155 average_feature_nn.b_ratio += b_ratio; 00156 } 00157 else 00158 { 00159 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); 00160 } 00161 } 00162 } 00163 00164 float normalization_factor = static_cast<float> (nn_indices.size ()); 00165 average_feature_nn.f1 /= normalization_factor; 00166 average_feature_nn.f2 /= normalization_factor; 00167 average_feature_nn.f3 /= normalization_factor; 00168 average_feature_nn.f4 /= normalization_factor; 00169 average_feature_nn.r_ratio /= normalization_factor; 00170 average_feature_nn.g_ratio /= normalization_factor; 00171 average_feature_nn.b_ratio /= normalization_factor; 00172 output.points[index_i] = average_feature_nn; 00173 } 00174 PCL_INFO ("Output size: %u\n", output.points.size ()); 00175 } 00176 00177 00178 #define PCL_INSTANTIATE_PPFRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBEstimation<T,NT,OutT>; 00179 #define PCL_INSTANTIATE_PPFRGBRegionEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBRegionEstimation<T,NT,OutT>; 00180 00181 #endif // PCL_FEATURES_IMPL_PPFRGB_H_