Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef NDTFRAMEPROC_HH
00036 #define NDTFRAMEPROC_HH
00037
00038 #include <ndt_feature_reg/ndt_frame.h>
00039 #include <ndt_matcher_d2d_feature.h>
00040 #include <opencv2/nonfree/features2d.hpp>
00041 #include <opencv2/nonfree/nonfree.hpp>
00042
00043 namespace ndt_feature_reg
00044 {
00045 template <typename PointT>
00046 class NDTFrameProc
00047 {
00048 public:
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 void addFrame (NDTFrame<PointT> *f)
00060 {
00061 frames.push_back(f);
00062 }
00063
00064 void addFrameIncremental (NDTFrame<PointT> *f, bool skipMatching, bool ndtEstimateDI = false,
00065 bool match_full = false, bool match_no_association = false);
00066 void trimNbFrames (size_t maxNbFrames);
00067
00068 void processFrames (bool skipMatching, bool ndtEstimateDI = false,
00069 bool match_full = false, bool match_no_association = false);
00070
00071 pcl::PointCloud<pcl::PointXYZRGB>::Ptr createColoredFeaturePC(NDTFrame<PointT> &f, pcl::PointXYZRGB color);
00072
00073 NDTFrameProc(int nb_ransac, double max_inldist_xy, double max_inldist_z): pe(nb_ransac, max_inldist_xy, max_inldist_z)
00074 {
00075 detector = cv::FeatureDetector::create("SURF");
00076 extractor = cv::DescriptorExtractor::create("SURF");
00077 img_scale = 1.;
00078 trim_factor = 1.;
00079 non_mean = false;
00080 cv::initModule_nonfree();
00081 cv::initModule_features2d();
00082
00083 }
00084
00085 PoseEstimator<PointT,PointT> pe;
00086 typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> EigenTransform;
00087 typename std::vector< NDTFrame<PointT>*,Eigen::aligned_allocator<NDTFrame<PointT> > > frames;
00088 typename std::vector<EigenTransform, Eigen::aligned_allocator<EigenTransform> > transformVector;
00089
00090 inline void convertMatches(const std::vector<cv::DMatch> &in, std::vector<std::pair<int,int> > &out)
00091 {
00092 out.resize(in.size());
00093 for (size_t i = 0; i < in.size(); i++)
00094 {
00095 out[i].first = in[i].queryIdx;
00096 out[i].second = in[i].trainIdx;
00097 }
00098 }
00099
00100 inline std::vector<std::pair<int,int> > convertMatches(const std::vector<cv::DMatch> &in)
00101 {
00102 std::vector<std::pair<int,int> > out;
00103 convertMatches(in,out);
00104 return out;
00105 }
00106
00107 cv::Ptr<cv::FeatureDetector> detector;
00108 cv::Ptr<cv::DescriptorExtractor> extractor;
00109 double img_scale;
00110 double trim_factor;
00111 bool non_mean;
00112 virtual ~NDTFrameProc()
00113 {
00114 for(size_t i =0; i<frames.size(); i++)
00115 {
00116 delete frames[i];
00117 }
00118 frames.clear();
00119 }
00120 private:
00121
00122 void detectKeypoints(NDTFrame<PointT> *f) const;
00123 void calcDescriptors(NDTFrame<PointT> *f) const;
00124 public:
00125 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00126
00127
00128
00129
00130 };
00131
00132
00133
00134 }
00135
00136 #include <ndt_feature_reg/impl/ndt_frame_proc.hpp>
00137 #endif