ppf.hpp
Go to the documentation of this file.
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 
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 
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:31:12