ndt_frame_proc.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of AASS Research Center nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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 //        bool loadImg(NDTFrame<PointT> &f, const std::string &fileName) const;
00050 //        void loadPC(NDTFrame<PointT> &f, const std::string &fileName) const;
00051 
00052 //        bool loadPCfromDepth(NDTFrame<PointT> &f, const std::string &fileName) const;
00053 
00054 //        bool setupDepthToPC(const std::string &fileName, double fx, double fy, double cx, double cy, const std::vector<double> &dist, double ds);
00055 //        void setupDepthToPC(const cv::Mat &depthImg, double fx, double fy, double cx, double cy, const std::vector<double> &dist, double ds);
00056 
00057 //        void convertDepthToPC(const cv::Mat &depthImg, pcl::PointCloud<PointT> &pc) const;
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 //        lslgeneric::NDTMatcherFeatureD2D<PointT,PointT> *matcher_feat; //(corr);
00128 
00129 
00130 };
00131 
00132 
00133 
00134 } // namespace
00135 
00136 #include <ndt_feature_reg/impl/ndt_frame_proc.hpp>
00137 #endif


ndt_feature_reg
Author(s): Henrik Andreasson, Todor Stoyanov
autogenerated on Mon Jan 6 2014 11:36:19