Go to the documentation of this file.00001
00059
00060
00061
00062
00063
00064
00065
00066 #ifndef MOMENTS_H_
00067 #define MOMENTS_H_
00068
00069 #include "../feature_container.h"
00070 #include <pcl/features/moment_invariants.h>
00071 #include <pcl/features/impl/moment_invariants.hpp>
00072 #ifdef GICP_ENABLE
00073 #include <pcl/search/kdtree.h>
00074 #include <pcl/kdtree/kdtree_flann.h>
00075 #else
00076 #include <pcl/kdtree/kdtree.h>
00077 #endif
00078
00079 template<typename Point>
00080 class Feature_MomentInvariants : public FeatureContainerInterface_Euclidean<Point>
00081 {
00082 float radius_;
00083
00084 pcl::PointCloud<pcl::MomentInvariants> feature_in_, feature_out_;
00085
00086 public:
00087 Feature_MomentInvariants():
00088 radius_(0.1)
00089 {
00090 }
00091
00092 void setMomentRadius(float v) {radius_ = v;}
00093
00094 virtual bool hidden_build() {
00095
00096 ROS_INFO("calc moments for source");
00097 {
00098 pcl::MomentInvariantsEstimation<Point,pcl::MomentInvariants> est;
00099 #ifdef GICP_ENABLE
00100 boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00101 #else
00102 #ifdef PCL_VERSION_COMPARE
00103 boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00104 #else
00105 boost::shared_ptr<pcl::KdTree<Point> > tree (new pcl::KdTreeFLANN<Point>);
00106 #endif
00107 #endif
00108
00109 est.setInputCloud(this->org_in_.makeShared());
00110 est.setSearchMethod(tree);
00111 est.setRadiusSearch(radius_);
00112
00113 est.compute(feature_in_);
00114 }
00115
00116 ROS_INFO("calc moments for target");
00117 {
00118 pcl::MomentInvariantsEstimation<Point,pcl::MomentInvariants> est;
00119 #ifdef GICP_ENABLE
00120 boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00121 #else
00122 #ifdef PCL_VERSION_COMPARE
00123 boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00124 #else
00125 boost::shared_ptr<pcl::KdTree<Point> > tree (new pcl::KdTreeFLANN<Point>);
00126 #endif
00127 #endif
00128
00129 est.setInputCloud(this->org_out_.makeShared());
00130 est.setSearchMethod(tree);
00131 est.setRadiusSearch(radius_);
00132
00133 est.compute(feature_out_);
00134 }
00135
00136 return true;
00137 }
00138
00139 virtual Eigen::VectorXf getFeatureOut(const int index) {
00140 Eigen::Vector3f b;
00141 b(0) = feature_out_.points[index].j1;
00142 b(1) = feature_out_.points[index].j2;
00143 b(2) = feature_out_.points[index].j3;
00144 return b;
00145 }
00146 virtual Eigen::VectorXf getFeatureIn(const int index) {
00147 Eigen::Vector3f b;
00148 b(0) = feature_in_.points[index].j1;
00149 b(1) = feature_in_.points[index].j2;
00150 b(2) = feature_in_.points[index].j3;
00151 return b;
00152 }
00153
00154 };
00155
00156
00157 #endif