-
-
Notifications
You must be signed in to change notification settings - Fork 4.6k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #296 from mszarski/cppf-impl
CPPF Feature Implementation per issue #280
- Loading branch information
Showing
9 changed files
with
550 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,120 @@ | ||
| /* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Point Cloud Library (PCL) - www.pointclouds.org | ||
| * Copyright (c) 2011, Alexandru-Eugen Ichim | ||
| * Copyright (c) 2012-, Open Perception, Inc. | ||
| * Copyright (c) 2013, Martin Szarski | ||
| * | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of the copyright holder(s) nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| */ | ||
|
|
||
| #ifndef PCL_CPPF_H_ | ||
| #define PCL_CPPF_H_ | ||
|
|
||
| #include <pcl/features/feature.h> | ||
| #include <pcl/features/boost.h> | ||
|
|
||
| namespace pcl | ||
| { | ||
| /** \brief | ||
| * \param[in] p1 | ||
| * \param[in] n1 | ||
| * \param[in] p2 | ||
| * \param[in] n2 | ||
| * \param[in] c1 | ||
| * \param[in] c2 | ||
| * \param[out] f1 | ||
| * \param[out] f2 | ||
| * \param[out] f3 | ||
| * \param[out] f4 | ||
| * \param[out] f5 | ||
| * \param[out] f6 | ||
| * \param[out] f7 | ||
| * \param[out] f8 | ||
| * \param[out] f9 | ||
| * \param[out] f10 | ||
| */ | ||
| PCL_EXPORTS bool | ||
| computeCPPFPairFeature (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &c1, | ||
| const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &c2, | ||
| float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7, float &f8, float &f9, float &f10); | ||
|
|
||
|
|
||
|
|
||
| /** \brief Class that calculates the "surflet" features for each pair in the given | ||
| * pointcloud. Please refer to the following publication for more details: | ||
| * C. Choi, Henrik Christensen | ||
| * 3D Pose Estimation of Daily Objects Using an RGB-D Camera | ||
| * Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) | ||
| * 2012 | ||
| * | ||
| * PointOutT is meant to be pcl::CPPFSignature - contains the 10 values of the Surflet | ||
| * feature and in addition, alpha_m for the respective pair - optimization proposed by | ||
| * the authors (see above) | ||
| * | ||
| * \author Martin Szarski, Alexandru-Eugen Ichim | ||
| */ | ||
|
|
||
| template <typename PointInT, typename PointNT, typename PointOutT> | ||
| class CPPFEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT> | ||
| { | ||
| public: | ||
| typedef boost::shared_ptr<CPPFEstimation<PointInT, PointNT, PointOutT> > Ptr; | ||
| typedef boost::shared_ptr<const CPPFEstimation<PointInT, PointNT, PointOutT> > ConstPtr; | ||
| using PCLBase<PointInT>::indices_; | ||
| using Feature<PointInT, PointOutT>::input_; | ||
| using Feature<PointInT, PointOutT>::feature_name_; | ||
| using Feature<PointInT, PointOutT>::getClassName; | ||
| using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_; | ||
|
|
||
| typedef pcl::PointCloud<PointOutT> PointCloudOut; | ||
|
|
||
| /** \brief Empty Constructor. */ | ||
| CPPFEstimation (); | ||
|
|
||
|
|
||
| private: | ||
| /** \brief The method called for actually doing the computations | ||
| * \param[out] output the resulting point cloud (which should be of type pcl::CPPFSignature); | ||
| * its size is the size of the input cloud, squared (i.e., one point for each pair in | ||
| * the input cloud); | ||
| */ | ||
| void | ||
| computeFeature (PointCloudOut &output); | ||
| }; | ||
| } | ||
|
|
||
| #ifdef PCL_NO_PRECOMPILE | ||
| #include <pcl/features/impl/cppf.hpp> | ||
| #endif | ||
|
|
||
| #endif // PCL_CPPF_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,124 @@ | ||
| /* | ||
| * Software License Agreement (BSD License) | ||
| * | ||
| * Point Cloud Library (PCL) - www.pointclouds.org | ||
| * Copyright (c) 2011, Alexandru-Eugen Ichim | ||
| * Copyright (c) 2012-, Open Perception, Inc. | ||
| * Copyright (c) 2013, Martin Szarski | ||
| * | ||
| * All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * * Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * * Redistributions in binary form must reproduce the above | ||
| * copyright notice, this list of conditions and the following | ||
| * disclaimer in the documentation and/or other materials provided | ||
| * with the distribution. | ||
| * * Neither the name of the copyright holder(s) nor the names of its | ||
| * contributors may be used to endorse or promote products derived | ||
| * from this software without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| * $Id$ | ||
| */ | ||
|
|
||
| #ifndef PCL_FEATURES_IMPL_CPPF_H_ | ||
| #define PCL_FEATURES_IMPL_CPPF_H_ | ||
|
|
||
| #include <pcl/features/cppf.h> | ||
| #include <pcl/features/pfh.h> | ||
|
|
||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| template <typename PointInT, typename PointNT, typename PointOutT> | ||
| pcl::CPPFEstimation<PointInT, PointNT, PointOutT>::CPPFEstimation () | ||
| : FeatureFromNormals <PointInT, PointNT, PointOutT> () | ||
| { | ||
| feature_name_ = "CPPFEstimation"; | ||
| // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute () | ||
| Feature<PointInT, PointOutT>::tree_.reset (new pcl::search::KdTree <PointInT> ()); | ||
| Feature<PointInT, PointOutT>::search_radius_ = 1.0f; | ||
| } | ||
|
|
||
|
|
||
| ////////////////////////////////////////////////////////////////////////////////////////////// | ||
| template <typename PointInT, typename PointNT, typename PointOutT> void | ||
| pcl::CPPFEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) | ||
| { | ||
| // Initialize output container - overwrite the sizes done by Feature::initCompute () | ||
| output.points.resize (indices_->size () * input_->points.size ()); | ||
| output.height = 1; | ||
| output.width = static_cast<uint32_t> (output.points.size ()); | ||
| output.is_dense = true; | ||
|
|
||
| // Compute point pair features for every pair of points in the cloud | ||
| for (size_t index_i = 0; index_i < indices_->size (); ++index_i) | ||
| { | ||
| size_t i = (*indices_)[index_i]; | ||
| for (size_t j = 0 ; j < input_->points.size (); ++j) | ||
| { | ||
| PointOutT p; | ||
| if (i != j) | ||
| { | ||
| if ( | ||
| pcl::computeCPPFPairFeature (input_->points[i].getVector4fMap (), | ||
| normals_->points[i].getNormalVector4fMap (), | ||
| input_->points[i].getRGBVector4i (), | ||
| input_->points[j].getVector4fMap (), | ||
| normals_->points[j].getNormalVector4fMap (), | ||
| input_->points[j].getRGBVector4i (), | ||
| p.f1, p.f2, p.f3, p.f4, p.f5, p.f6, p.f7, p.f8, p.f9, p.f10)) | ||
| { | ||
| // Calculate alpha_m angle | ||
| Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (), | ||
| model_reference_normal = normals_->points[i].getNormalVector3fMap (), | ||
| model_point = input_->points[j].getVector3fMap (); | ||
| Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), | ||
| model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ()); | ||
| Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg; | ||
|
|
||
| Eigen::Vector3f model_point_transformed = transform_mg * model_point; | ||
| float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1)); | ||
| if (sin (angle) * model_point_transformed(2) < 0.0f) | ||
| angle *= (-1); | ||
| p.alpha_m = -angle; | ||
| } | ||
| else | ||
| { | ||
| PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j); | ||
| p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits<float>::quiet_NaN (); | ||
| output.is_dense = false; | ||
| } | ||
| } | ||
| // Do not calculate the feature for identity pairs (i, i) as they are not used | ||
| // in the following computations | ||
| else | ||
| { | ||
| p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits<float>::quiet_NaN (); | ||
| output.is_dense = false; | ||
| } | ||
|
|
||
| output.points[index_i*input_->points.size () + j] = p; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| #define PCL_INSTANTIATE_CPPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CPPFEstimation<T,NT,OutT>; | ||
|
|
||
|
|
||
| #endif // PCL_FEATURES_IMPL_CPPF_H_ |
Oops, something went wrong.