ndt_frame_proc.hpp
Go to the documentation of this file.
00001 #include <highgui.h>
00002 #include <Eigen/Eigen>
00003 
00004 namespace ndt_feature_reg
00005 {
00006 
00007 template <typename PointT>
00008 void NDTFrameProc<PointT>::addFrameIncremental (NDTFrame<PointT> *f, bool skipMatching, bool ndtEstimateDI,
00009         bool match_full, bool match_no_association)
00010 {
00011 
00012     if(!match_full)
00013     {
00014         detectKeypoints(f);
00015     }
00016 
00017     f->computeNDT(ndtEstimateDI, this->non_mean);
00018 
00019     if(!(match_no_association||match_full))
00020     {
00021         f->assignPts();
00022         calcDescriptors(f);
00023     }
00024     frames.push_back(f);
00025 
00026     NDTFrameProc<PointT>::EigenTransform tr;
00027     tr.setIdentity();
00028     if(frames.size() == 1)
00029     {
00030 
00031         transformVector.clear();
00032         transformVector.push_back(tr);
00033         return;
00034     }
00035 
00036 
00037     Eigen::Affine3d transl_transform;
00038     Eigen::Affine3d rot_transform;
00039     std::vector<std::pair<int,int> > corr;
00040 
00041     int i = frames.size()-1;
00042 
00043     if(!(match_full || match_no_association))
00044     {
00045         pe.estimate(*frames[i-1],*frames[i]);
00046         transl_transform = (Eigen::Affine3d)Eigen::Translation3d(pe.trans);
00047         rot_transform = (Eigen::Affine3d)Eigen::Matrix3d(pe.rot);
00048         tr = transl_transform*rot_transform;
00049         if (!skipMatching)
00050         {
00051             corr.clear();
00052             corr = convertMatches(pe.inliers);
00053             lslgeneric::NDTMatcherFeatureD2D<PointT,PointT> matcher_feat(corr, trim_factor);
00054             matcher_feat.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr, true); //true = use initial guess
00055             //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
00056         }
00057     }
00058     else
00059     {
00060         lslgeneric::NDTMatcherD2D<PointT,PointT> matcher;
00061         matcher.current_resolution = frames[i-1]->current_res/2;
00062         matcher.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr);
00063         //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
00064     }
00065 
00066 
00067     transformVector.push_back(tr);
00068 
00069 }
00070 
00071 template <typename PointT>
00072 void NDTFrameProc<PointT>::trimNbFrames (size_t maxNbFrames)
00073 {
00074     if (frames.size() > maxNbFrames)
00075     {
00076         //frames.erase(frames.begin(), frames.begin() + frames.size() - maxNbFrames);
00077         for(size_t i =0; i<frames.size()-1; i++)
00078         {
00079             //std::cout<<"delete f : "<<i<<std::endl;
00080             delete frames[i];
00081         }
00082         frames.erase(frames.begin(), frames.end() - 1);
00083         transformVector.erase(transformVector.begin(), transformVector.end() - 1); // Just to keep a 1-1 coorespondance with frames.
00084     }
00085 }
00086 
00087 template <typename PointT>
00088 void NDTFrameProc<PointT>::processFrames (bool skipMatching, bool ndtEstimateDI,
00089         bool match_full, bool match_no_association)
00090 {
00091     if(frames.size() < 1) return;
00092 
00093     transformVector.clear();
00094     NDTFrameProc<PointT>::EigenTransform tr;
00095     tr.setIdentity();
00096     transformVector.push_back(tr);
00097 
00098     if(!match_full)
00099     {
00100         detectKeypoints(frames[0]);
00101     }
00102     frames[0]->computeNDT(ndtEstimateDI, this->non_mean);
00103 
00104     if(!(match_no_association||match_full))
00105     {
00106         frames[0]->assignPts();
00107         calcDescriptors(frames[0]);
00108     }
00109 
00110     Eigen::Affine3d transl_transform;
00111     Eigen::Affine3d rot_transform;
00112     std::vector<std::pair<int,int> > corr;
00113 
00114     for(size_t i=1; i<frames.size(); i++)
00115     {
00116 
00117         if(!match_full)
00118         {
00119             detectKeypoints(frames[i]);
00120         }
00121         frames[i]->computeNDT(ndtEstimateDI, this->non_mean);
00122         if(!(match_no_association||match_full))
00123         {
00124             frames[i]->assignPts();
00125             calcDescriptors(frames[i]);
00126 
00127             pe.estimate(*frames[i-1],*frames[i]);
00128             transl_transform = (Eigen::Affine3d)Eigen::Translation3d(pe.trans);
00129             rot_transform = (Eigen::Affine3d)Eigen::Matrix3d(pe.rot);
00130             tr = transl_transform*rot_transform;
00131 
00132             if (!skipMatching)
00133             {
00134                 corr.clear();
00135                 corr = convertMatches(pe.inliers);
00136                 lslgeneric::NDTMatcherFeatureD2D<PointT,PointT> matcher_feat(corr, trim_factor);
00137                 matcher_feat.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr, true); //true = use initial guess
00138                 //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
00139             }
00140         }
00141         else
00142         {
00143             lslgeneric::NDTMatcherD2D<PointT,PointT> matcher;
00144             matcher.current_resolution = frames[i-1]->current_res/2;
00145             matcher.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr);
00146             //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
00147         }
00148 
00149         /*
00150                 transformVector.push_back(tr);
00151                 cv::Mat display;
00152                 drawMatches(frames[i-1]->img, frames[i-1]->kpts, frames[i]->img, frames[i]->kpts, pe.inliers, display);
00153                 const std::string window_name = "matches";
00154                 cv::namedWindow(window_name,0);
00155                 cv::imshow(window_name, display);
00156                 cv::waitKey(0);
00157         */
00158 
00159     }
00160 
00161 }
00162 
00163 template <typename PointT>
00164 void
00165 NDTFrameProc<PointT>::detectKeypoints(NDTFrame<PointT> *f) const
00166 {
00167     if (img_scale == 1.)
00168     {
00169         detector->detect(f->img, f->kpts);
00170     }
00171     else
00172     {
00173         cv::Mat tmp;
00174         cv::resize(f->img, tmp, cv::Size(), img_scale, img_scale, cv::INTER_LINEAR);
00175         detector->detect(tmp, f->kpts);
00176         float factor = 1./img_scale;
00177         scaleKeyPointPosition(f->kpts, factor);
00178     }
00179 }
00180 
00181 template <typename PointT>
00182 void
00183 NDTFrameProc<PointT>::calcDescriptors(NDTFrame<PointT> *f) const
00184 {
00185     if (img_scale == 1.)
00186     {
00187         extractor->compute(f->img, f->kpts, f->dtors);
00188     }
00189     else
00190     {
00191         cv::Mat tmp;
00192         cv::resize(f->img, tmp, cv::Size(), img_scale, img_scale, cv::INTER_LINEAR);
00193         float factor = img_scale;
00194         scaleKeyPointPosition(f->kpts, factor);
00195         extractor->compute(tmp, f->kpts, f->dtors);
00196         factor = 1./img_scale;
00197         scaleKeyPointPosition(f->kpts, factor);
00198     }
00199 }
00200 
00201 template <typename PointT>
00202 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00203 NDTFrameProc<PointT>::createColoredFeaturePC(NDTFrame<PointT> &f, pcl::PointXYZRGB color)
00204 {
00205     pcl::PointCloud<pcl::PointXYZRGB>::Ptr ret(new pcl::PointCloud<pcl::PointXYZRGB>());
00206     *ret = f.getColoredPointCloud();
00207     size_t w = f.img.size().width;
00208     //size_t h = f.img.size().height;
00209     size_t idx = 0;
00210 
00211     for (size_t i = 0; i < f.kpts.size(); i++)
00212     {
00213         int uKey = static_cast<int>(f.kpts[i].pt.x+0.5);
00214         int vKey = static_cast<int>(f.kpts[i].pt.y+0.5);
00215 
00216         idx = vKey * w + uKey;
00217         pcl::PointXYZRGB& pt = ret->points[idx];
00218         pt.rgb = color.rgb;
00219     }
00220     return ret;
00221 }
00222 
00223 /*
00224 template <typename PointT>
00225 bool
00226 NDTFrameProc<PointT>::loadImg(NDTFrame<PointT> &f, const std::string &fileName) const
00227 {
00228      f.img = cv::imread(fileName, 0);
00229      if (f.img.empty())
00230      {
00231          std::cerr << "Failed to load : " << fileName << std::endl;
00232           return false;
00233      }
00234      return true;
00235 }
00236 
00237 template <typename PointT>
00238 void
00239 NDTFrameProc<PointT>::loadPC(NDTFrame<PointT> &f, const std::string &fileName) const
00240 {
00241 
00242 }
00243 
00244 template <typename PointT>
00245 bool
00246 NDTFrameProc<PointT>::loadPCfromDepth(NDTFrame<PointT> &f, const std::string &fileName) const
00247 {
00248      // Load the file.
00249      f.depth_img = cv::imread(fileName, CV_LOAD_IMAGE_ANYDEPTH); // CV_LOAD_IMAGE_ANYDEPTH is important to load the 16bits image
00250      if (f.depth_img.empty())
00251      {
00252           std::cerr << "Failed to load : " << fileName << std::endl;
00253      }
00254      if (!this->_lookupTable.empty())
00255      {
00256           //convertDepthToPC(f.depth_img, f.pc);
00257           return true;
00258      }
00259      return false;
00260 }
00261 
00262 template <typename PointT>
00263 bool
00264 NDTFrameProc<PointT>::setupDepthToPC(const std::string &fileName, double fx, double fy, double cx, double cy, const std::vector<double> &dist, double ds)
00265 {
00266      cv::Mat depth_img = cv::imread(fileName, CV_LOAD_IMAGE_ANYDEPTH);
00267      if (depth_img.empty())
00268      {
00269           std::cerr << "Failed to load : " << fileName << std::endl;
00270           return false;
00271      }
00272      setupDepthToPC(depth_img, fx, fy, cx, cy, dist, ds);
00273      return true;
00274 }
00275 
00276 template <typename PointT>
00277 void
00278 NDTFrameProc<PointT>::setupDepthToPC(const cv::Mat &depthImg, double fx, double fy, double cx, double cy, const std::vector<double> &dist, double ds)
00279 {
00280      _camMat = getCameraMatrix(fx, fy, cx, cy);
00281      _dist = getDistVector(dist[0], dist[1], dist[2], dist[3], dist[4]);
00282 
00283      _lookupTable = getDepthPointCloudLookUpTable(depthImg.size(), _camMat, _dist, ds);
00284 }
00285 
00286 template <typename PointT>
00287 void
00288 NDTFrameProc<PointT>::convertDepthToPC(const cv::Mat &depthImg, pcl::PointCloud<PointT> &pc) const
00289 {
00290      convertDepthImageToPointCloud(depthImg, _lookupTable, pc);
00291 }
00292 */
00293 }


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