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) 2011, Alexandru-Eugen Ichim 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$ 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_PPF_H_ 00041 #define PCL_FEATURES_IMPL_PPF_H_ 00042 00043 #include <pcl/features/ppf.h> 00044 #include <pcl/features/pfh.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointInT, typename PointNT, typename PointOutT> 00048 pcl::PPFEstimation<PointInT, PointNT, PointOutT>::PPFEstimation () 00049 : FeatureFromNormals <PointInT, PointNT, PointOutT> () 00050 { 00051 feature_name_ = "PPFEstimation"; 00052 // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () 00053 Feature<PointInT, PointOutT>::tree_.reset (new pcl::search::KdTree <PointInT> ()); 00054 Feature<PointInT, PointOutT>::search_radius_ = 1.0f; 00055 } 00056 00057 00058 ////////////////////////////////////////////////////////////////////////////////////////////// 00059 template <typename PointInT, typename PointNT, typename PointOutT> void 00060 pcl::PPFEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00061 { 00062 // Initialize output container - overwrite the sizes done by Feature::initCompute () 00063 output.points.resize (indices_->size () * input_->points.size ()); 00064 output.height = 1; 00065 output.width = static_cast<uint32_t> (output.points.size ()); 00066 output.is_dense = true; 00067 00068 // Compute point pair features for every pair of points in the cloud 00069 for (size_t index_i = 0; index_i < indices_->size (); ++index_i) 00070 { 00071 size_t i = (*indices_)[index_i]; 00072 for (size_t j = 0 ; j < input_->points.size (); ++j) 00073 { 00074 PointOutT p; 00075 if (i != j) 00076 { 00077 if (//pcl::computePPFPairFeature 00078 pcl::computePairFeatures (input_->points[i].getVector4fMap (), 00079 normals_->points[i].getNormalVector4fMap (), 00080 input_->points[j].getVector4fMap (), 00081 normals_->points[j].getNormalVector4fMap (), 00082 p.f1, p.f2, p.f3, p.f4)) 00083 { 00084 // Calculate alpha_m angle 00085 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), 00086 model_reference_normal = normals_->points[i].getNormalVector3fMap (), 00087 model_point = input_->points[j].getVector3fMap (); 00088 Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), 00089 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); 00090 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; 00091 00092 Eigen::Vector3f model_point_transformed = transform_mg * model_point; 00093 float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); 00094 if (sin (angle) * model_point_transformed(2) < 0.0f) 00095 angle *= (-1); 00096 p.alpha_m = -angle; 00097 } 00098 else 00099 { 00100 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); 00101 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits<float>::quiet_NaN (); 00102 output.is_dense = false; 00103 } 00104 } 00105 // Do not calculate the feature for identity pairs (i, i) as they are not used 00106 // in the following computations 00107 else 00108 { 00109 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits<float>::quiet_NaN (); 00110 output.is_dense = false; 00111 } 00112 00113 output.points[index_i*input_->points.size () + j] = p; 00114 } 00115 } 00116 } 00117 00118 #define PCL_INSTANTIATE_PPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFEstimation<T,NT,OutT>; 00119 00120 00121 #endif // PCL_FEATURES_IMPL_PPF_H_