moment_invariants.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) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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  * $Id: moment_invariants.hpp 5026 2012-03-12 02:51:44Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_
00041 #define PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_
00042 
00043 #include <pcl/features/moment_invariants.h>
00044 
00046 template <typename PointInT, typename PointOutT> void
00047 pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computePointMomentInvariants (
00048       const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
00049       float &j1, float &j2, float &j3)
00050 {
00051   // Estimate the XYZ centroid
00052   compute3DCentroid (cloud, indices, xyz_centroid_);
00053 
00054   // Initalize the centralized moments
00055   float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011  = 0;
00056 
00057   // Iterate over the nearest neighbors set
00058   for (size_t nn_idx = 0; nn_idx < indices.size (); ++nn_idx)
00059   {
00060     // Demean the points
00061     temp_pt_[0] = cloud.points[indices[nn_idx]].x - xyz_centroid_[0];
00062     temp_pt_[1] = cloud.points[indices[nn_idx]].y - xyz_centroid_[1];
00063     temp_pt_[2] = cloud.points[indices[nn_idx]].z - xyz_centroid_[2];
00064 
00065     mu200 += temp_pt_[0] * temp_pt_[0];
00066     mu020 += temp_pt_[1] * temp_pt_[1];
00067     mu002 += temp_pt_[2] * temp_pt_[2];
00068     mu110 += temp_pt_[0] * temp_pt_[1];
00069     mu101 += temp_pt_[0] * temp_pt_[2];
00070     mu011 += temp_pt_[1] * temp_pt_[2];
00071   }
00072 
00073   // Save the moment invariants
00074   j1 = mu200             + mu020               + mu002;
00075   j2 = mu200*mu020       + mu200*mu002         + mu020*mu002       - mu110*mu110       - mu101*mu101       - mu011*mu011;
00076   j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
00077 }
00078 
00080 template <typename PointInT, typename PointOutT> void
00081 pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computePointMomentInvariants (
00082       const pcl::PointCloud<PointInT> &cloud, float &j1, float &j2, float &j3)
00083 {
00084   // Estimate the XYZ centroid
00085   compute3DCentroid (cloud, xyz_centroid_);
00086 
00087   // Initalize the centralized moments
00088   float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011  = 0;
00089 
00090   // Iterate over the nearest neighbors set
00091   for (size_t nn_idx = 0; nn_idx < cloud.points.size (); ++nn_idx )
00092   {
00093     // Demean the points
00094     temp_pt_[0] = cloud.points[nn_idx].x - xyz_centroid_[0];
00095     temp_pt_[1] = cloud.points[nn_idx].y - xyz_centroid_[1];
00096     temp_pt_[2] = cloud.points[nn_idx].z - xyz_centroid_[2];
00097 
00098     mu200 += temp_pt_[0] * temp_pt_[0];
00099     mu020 += temp_pt_[1] * temp_pt_[1];
00100     mu002 += temp_pt_[2] * temp_pt_[2];
00101     mu110 += temp_pt_[0] * temp_pt_[1];
00102     mu101 += temp_pt_[0] * temp_pt_[2];
00103     mu011 += temp_pt_[1] * temp_pt_[2];
00104   }
00105 
00106   // Save the moment invariants
00107   j1 = mu200             + mu020               + mu002;
00108   j2 = mu200*mu020       + mu200*mu002         + mu020*mu002       - mu110*mu110       - mu101*mu101       - mu011*mu011;
00109   j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
00110 }
00111 
00113 template <typename PointInT, typename PointOutT> void
00114 pcl::MomentInvariantsEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00115 {
00116   // Allocate enough space to hold the results
00117   // \note This resize is irrelevant for a radiusSearch ().
00118   std::vector<int> nn_indices (k_);
00119   std::vector<float> nn_dists (k_);
00120 
00121   output.is_dense = true;
00122   // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
00123   if (input_->is_dense)
00124   {
00125     // Iterating over the entire index vector
00126     for (size_t idx = 0; idx < indices_->size (); ++idx)
00127     {
00128       if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00129       {
00130         output.points[idx].j1 = output.points[idx].j2 = output.points[idx].j3 = std::numeric_limits<float>::quiet_NaN ();
00131         output.is_dense = false;
00132         continue;
00133       }
00134      
00135       computePointMomentInvariants (*surface_, nn_indices,
00136                                     output.points[idx].j1, output.points[idx].j2, output.points[idx].j3);
00137     }
00138   }
00139   else
00140   {
00141     // Iterating over the entire index vector
00142     for (size_t idx = 0; idx < indices_->size (); ++idx)
00143     {
00144       if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00145           this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00146       {
00147         output.points[idx].j1 = output.points[idx].j2 = output.points[idx].j3 = std::numeric_limits<float>::quiet_NaN ();
00148         output.is_dense = false;
00149         continue;
00150       }
00151 
00152       computePointMomentInvariants (*surface_, nn_indices,
00153                                     output.points[idx].j1, output.points[idx].j2, output.points[idx].j3);
00154     }
00155   }
00156 }
00157 
00159 template <typename PointInT> void
00160 pcl::MomentInvariantsEstimation<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00161 {
00162   // Resize the output dataset
00163   output.points.resize (indices_->size (), 3);
00164 
00165   // Allocate enough space to hold the results
00166   // \note This resize is irrelevant for a radiusSearch ().
00167   std::vector<int> nn_indices (k_);
00168   std::vector<float> nn_dists (k_);
00169 
00170   output.is_dense = true;
00171   // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
00172   if (input_->is_dense)
00173   {
00174     // Iterating over the entire index vector
00175     for (size_t idx = 0; idx < indices_->size (); ++idx)
00176     {
00177       if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00178       {
00179         output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = std::numeric_limits<float>::quiet_NaN ();
00180         output.is_dense = false;
00181         continue;
00182       }
00183 
00184       this->computePointMomentInvariants (*surface_, nn_indices,
00185                                     output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
00186     }
00187   }
00188   else
00189   {
00190     // Iterating over the entire index vector
00191     for (size_t idx = 0; idx < indices_->size (); ++idx)
00192     {
00193       if (!isFinite ((*input_)[(*indices_)[idx]]) ||
00194           this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
00195       {
00196         output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = std::numeric_limits<float>::quiet_NaN ();
00197         output.is_dense = false;
00198         continue;
00199        }
00200 
00201       this->computePointMomentInvariants (*surface_, nn_indices,
00202                                     output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
00203     }
00204   }
00205 }
00206 
00207 
00208 #define PCL_INSTANTIATE_MomentInvariantsEstimation(T,NT) template class PCL_EXPORTS pcl::MomentInvariantsEstimation<T,NT>;
00209 
00210 #endif    // PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ 
00211 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:44