Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * Willow Garage, Inc 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id: ppf.hpp 3022 2011-11-01 03:42:22Z rusu $ 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_PPF_H_ 00039 #define PCL_FEATURES_IMPL_PPF_H_ 00040 00041 #include "pcl/features/ppf.h" 00042 #include "pcl/features/pfh.h" 00043 00045 template <typename PointInT, typename PointNT, typename PointOutT> 00046 pcl::PPFEstimation<PointInT, PointNT, PointOutT>::PPFEstimation () 00047 : FeatureFromNormals <PointInT, PointNT, PointOutT> () 00048 { 00049 feature_name_ = "PPFEstimation"; 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 00057 template <typename PointInT, typename PointNT, typename PointOutT> void 00058 pcl::PPFEstimation<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 = 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::computePPFPairFeature 00075 pcl::computePairFeatures 00076 (input_->points[i].getVector4fMap (), 00077 normals_->points[i].getNormalVector4fMap (), 00078 input_->points[j].getVector4fMap (), 00079 normals_->points[j].getNormalVector4fMap (), 00080 p.f1, p.f2, p.f3, p.f4)) 00081 { 00082 // Calculate alpha_m angle 00083 Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), 00084 model_reference_normal = normals_->points[i].getNormalVector3fMap (), 00085 model_point = input_->points[j].getVector3fMap (); 00086 Eigen::AngleAxisf rotation_mg (acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), 00087 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); 00088 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; 00089 00090 Eigen::Vector3f model_point_transformed = transform_mg * model_point; 00091 float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); 00092 if (sin (angle) * model_point_transformed(2) < 0.0f) 00093 angle *= (-1); 00094 p.alpha_m = -angle; 00095 } 00096 else 00097 { 00098 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), (unsigned long) i, (unsigned long) j); 00099 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = 0.0; 00100 } 00101 } 00102 // Do not calculate the feature for identity pairs (i, i) as they are not used 00103 // in the following computations 00104 else 00105 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = 0.0; 00106 00107 output.points[index_i*input_->points.size () + j] = p; 00108 } 00109 } 00110 } 00111 00112 #define PCL_INSTANTIATE_PPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFEstimation<T,NT,OutT>; 00113 00114 00115 #endif // PCL_FEATURES_IMPL_PPF_H_