ndt_frame.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 NDTFRAME_HH
00036 #define NDTFRAME_HH
00037 
00038 #include "pcl/point_cloud.h"
00039 #include "pcl/point_types.h"
00040 #include "pcl/io/pcd_io.h"
00041 #include "pcl/features/feature.h"
00042 #include "pcl/registration/icp.h"
00043 #include "pcl/filters/voxel_grid.h"
00044 #include "opencv2/core/core.hpp"
00045 
00046 #include <cell_vector.h>
00047 #include <ndt_map.h>
00048 #include <depth_camera.h>
00049 
00050 #include <ndt_feature_reg/ndt_frame_tools.h>
00051 #include <pointcloud_utils.h>
00052 
00053 namespace ndt_feature_reg
00054 {
00055 inline
00056 double getDoubleTime(struct timeval& time)
00057 {
00058     return time.tv_sec + time.tv_usec * 1e-6;
00059 }
00060 
00061 inline
00062 double getDoubleTime()
00063 {
00064     struct timeval time;
00065     gettimeofday(&time,NULL);
00066     return getDoubleTime(time);
00067 }
00068 
00069 template <typename PointT>
00070 class NDTFrame
00071 {
00072 public:
00073     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00074 public:
00075     NDTFrame() : supportSize(3),maxVar(0.3),current_res(0.2)
00076     {
00077         ndt_map = lslgeneric::NDTMap<PointT>(&idx_prototype);
00078     }
00079     virtual ~NDTFrame()
00080     {
00081     }
00082     NDTFrame(const NDTFrame &other)
00083     {
00084         other.img.copyTo(img);
00085         other.depth_img.copyTo(depth_img);
00086         kpts = other.kpts;
00087         pc_kpts = other.pc_kpts;
00088         pts = other.pts;
00089         idx_prototype = other.idx_prototype;
00090         ndt_map = other.ndt_map;
00091         supportSize = other.supportSize;
00092         cameraParams = other.cameraParams;
00093         maxVar = other.maxVar;
00094         current_res=other.current_res;
00095         other.dtors.copyTo(dtors);
00096     }
00097 
00098     void clear()
00099     {
00100         maxVar = 0;
00101         kpts.resize(0);
00102         pc_kpts.resize(0);
00103         pts.resize(0);
00104         idx_prototype = lslgeneric::CellVector<PointT>();
00105         ndt_map = lslgeneric::NDTMap<PointT>(&idx_prototype);
00106 
00107         //pc.resize(0);
00108         //kpts_pc_indices.resize(0);
00109     }
00110 
00111     cv::Mat img;
00112     cv::Mat depth_img;
00113     size_t supportSize;
00114     double maxVar;
00115     double current_res;
00116     lslgeneric::DepthCamera<PointT> cameraParams;
00117     std::vector<cv::KeyPoint> kpts;
00118     pcl::PointCloud<PointT> pc_kpts; //cloud containing only keypoints
00120     std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > pts;
00121 
00122     lslgeneric::CellVector<PointT> idx_prototype;
00123     lslgeneric::NDTMap<PointT> ndt_map;
00124 
00125     const PointT& getKeyPointCenter(int keyPointIdx)
00126     {
00127         //const std::vector<size_t> &indices = kpts_pc_indices[keyPointIdx];
00128         const PointT &pt = pc_kpts[keyPointIdx]; // TODO - The first points is the centre only if the centre point is invalid...
00129         return pt;
00130     }
00131 
00132     void assignPts()
00133     {
00134         if(pc_kpts.size() == 0)
00135             computeNDT();
00136         pts.resize(pc_kpts.size());
00137         for (size_t i = 0; i < pc_kpts.size(); i++)
00138         {
00139             const PointT& pt = pc_kpts[i];
00140             pts[i].head(3) = Eigen::Vector3d(pt.x,pt.y,pt.z);
00141             pts[i](3) = 1.0;
00142             //std::cout<<"keypoint "<<i<<" at "<<pts[i].transpose()<<std::endl;
00143         }
00144     }
00145 
00146     void computeNDT(bool estimateDI = false, bool nonMean = false)
00147     {
00148         if(kpts.size() > 0)
00149         {
00150             //double t1 = getDoubleTime();
00151             pc_kpts = ndt_map.loadDepthImageFeatures(depth_img,kpts,supportSize,maxVar,cameraParams,estimateDI,nonMean);
00152             //double t2 = getDoubleTime();
00153             //std::cout<<"computing ndt took (features)"<<t2-t1<<std::endl;
00154         }
00155         else
00156         {
00157             lslgeneric::LazyGrid<PointT> idx_prototype_grid(current_res);
00158             ndt_map = lslgeneric::NDTMap<PointT>(&idx_prototype_grid);
00159             //double t1 = getDoubleTime();
00160             ndt_map.loadDepthImage(depth_img,cameraParams);
00161             ndt_map.computeNDTCells();
00162             //double t2 = getDoubleTime();
00163             //std::cout<<"computing ndt took (grid)"<<t2-t1<<std::endl;
00164 
00165         }
00166         //ndt_map.computeNDTCells();
00167         //double t3 = getDoubleTime();
00168 
00169         /*
00170          double t1 = getDoubleTime();
00171          selectKeyPointIndices();
00172          double t2 = getDoubleTime();
00173          //ndt_map.loadPointCloud(pc, kpts_pc_indices);
00174          double t3 = getDoubleTime();
00175          ndt_map.computeNDTCells();
00176          double t4 = getDoubleTime();
00177          std::cout << "selectKeyPointIndices : " << t2 - t1 << std::endl;
00178          std::cout << "loadPointCloud : " << t3 - t2 << std::endl;
00179          std::cout << "computeNDTCells : " << t4 - t3 << std::endl;
00180          std::cout << "kpts_pc_indices.size() : " << kpts_pc_indices.size() << std::endl;
00181         */
00182     }
00183     /*
00184     void createKeyPointCloud()
00185     {
00186         pc_kpts.clear();
00187         size_t sz = 1;
00188         pcl::PointCloud<PointT> pc_kpts_loc;
00189         std::vector<cv::KeyPoint> good_kpts;
00190         //lslgeneric::CellVector<PointT> *cv = dynamic_cast<lslgeneric::CellVector<PointT> *>(ndt_map->getMyIndex());
00191 
00192         for (size_t i = 0; i < kpts.size(); i++)
00193         {
00194       cameraParams.computePointsAtIndex(depth_img,kpts[i],sz,pc_kpts_loc);
00195       PointT pt = pc_kpts_loc[0];
00196       if(std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z))
00197         continue;
00198 
00199       good_kpts.push_back(kpts[i]);
00200       pc_kpts.push_back(pt);
00201         }
00202         kpts.clear();
00203         kpts.resize(0);
00204         kpts = good_kpts;
00205     } */
00206 
00207     pcl::PointCloud<pcl::PointXYZRGB> getColoredPointCloud()
00208     {
00209         lslgeneric::DepthCamera<pcl::PointXYZRGB> cameraParamsLocal (cameraParams.fx,cameraParams.fy,cameraParams.cx,
00210                 cameraParams.cy,cameraParams.dist,cameraParams.ds, cameraParams.isFloatImg);
00211         cameraParamsLocal.setupDepthPointCloudLookUpTable(depth_img.size());
00212 
00213         pcl::PointCloud<pcl::PointXYZRGB> cloud; //(new pcl::PointCloud<pcl::PointXYZRGB>);
00214         cameraParamsLocal.convertDepthImageToPointCloud(depth_img,cloud);
00215 
00216         size_t w = img.size().width;
00217         size_t h = img.size().height;
00218         size_t idx = 0;
00219         cloud.width = w;
00220         cloud.height = h;
00221         cloud.is_dense = true;
00222         const uchar* pimg = img.ptr<uchar>(0);
00223         std::cout<<"img channels "<<img.channels()<<std::endl;
00224         pcl::PointXYZRGB color;
00225         //int r=0, g=0, b=0;
00226         uint8_t r = 0, g = 0, b = 0;
00227 
00228         for( size_t i =0; i<h; i++)
00229         {
00230             for( size_t j =0; j<w; j++)
00231             {
00232                 if(img.channels() == 3)
00233                 {
00234                     const cv::Vec3b& bgr = img.at<cv::Vec3b>((int)i,(int)j);
00235                     r = bgr[0];
00236                     g = bgr[1];
00237                     b = bgr[2];
00238 
00239                     idx = 3*(i * w + j);
00240                     /*                  r = pimg[idx];
00241                                         g = pimg[idx+1];
00242                                         b = pimg[idx+2];*/
00243                 }
00244                 else if (img.channels() == 1)
00245                 {
00246                     idx = (i * w + j);
00247                     r = pimg[idx];
00248                     g = pimg[idx];
00249                     b = pimg[idx];
00250                 }
00251 
00252 //                  std::cout<<r<<" "<<g<<" "<<b<<std::endl;
00253 //                  uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
00254                 pcl::PointXYZRGB &color = cloud.points[idx];
00255                 //color.rgb = *reinterpret_cast<float*>(&rgb);
00256                 color.r = r;
00257                 color.g = g;
00258                 color.b = b;
00259 //                  std::cout<<color.r<<" "<<color.g<<" "<<color.b<<" "<<color.rgb<<std::endl;
00260 
00261 //                  cloud.at(j,i) = color;
00262                 /*
00263                                     color = getPCLColor(r,g,b);
00264                                     color.r = r;
00265                                     color.g = g;
00266                                     color.b = b;
00267                                     std::cout<<color.r<<" "<<color.g<<" "<<color.b<<" "<<color.rgb<<std::endl;
00268                                     cloud->at(j,i).rgb = color.rgb;
00269                                     cloud->at(j,i).r = r;
00270                                     cloud->at(j,i).g = g;
00271                                     cloud->at(j,i).b = b;
00272                 */
00273             }
00274         }
00275         return cloud;
00276     }
00277 
00278     cv::Mat dtors;
00279 
00280     //int nbPoints;
00281     //pcl::PointCloud<PointT> pc;
00282     //std::vector<std::vector<size_t> > kpts_pc_indices; //indeces of the keypoints inside the point cloud - not needed
00283     // Only to use with an initial alignment (simply a sparser NDTMap with non-grid Gaussians based on the keypoints).
00284     // Only used to define the spatial structure of the ndt_map.
00285 
00286     /*
00287           void removeFiniteIndices(std::vector<size_t> &indices)
00288                {
00289                     std::vector<size_t> tmp;
00290                     for (size_t i = 0; i < indices.size(); i++)
00291                     {
00292                          const PointT& pt = pc[indices[i]];
00293                          if (!std::isfinite(pt.x) ||
00294                              !std::isfinite(pt.y) ||
00295                              !std::isfinite(pt.z))
00296                          {
00297 
00298                          }
00299                          else
00300                          {
00301                               tmp.push_back(indices[i]);
00302                          }
00303                     }
00304                     indices = tmp;
00305                }
00306 
00307           void selectKeyPointIndices()
00308                {
00309                     size_t nkpts = kpts.size();
00310                     kpts_pc_indices.reserve(nkpts);
00311                     std::vector<cv::KeyPoint> good_kpts;
00312                     for (size_t i=0; i<nkpts; i++)
00313                     {
00314 
00315                          std::vector<size_t> indices;
00316                          selectIndicesAroundKeyPoint(kpts[i], pc, nbPoints, indices);
00317                          removeFiniteIndices(indices);
00318 
00319                          // Compute the NDTCell here...
00320                          lslgeneric::NDTCell<PointT> tmp;
00321                          for (size_t j = 0; j < indices.size(); j++)
00322                          {
00323                               tmp.points_.push_back(pc[indices[j]]);
00324                          }
00325                          tmp.computeGaussian();
00326                          if (tmp.hasGaussian_)
00327                          {
00328                               kpts_pc_indices.push_back(indices);
00329                               good_kpts.push_back(kpts[i]);
00330                               idx_prototype.addCell(tmp.copy());
00331                          }
00332                          else
00333                          {
00334     //                        assert(false);
00335                          }
00336                     }
00337                     kpts = good_kpts; // Probably a bit inefficient- but now we only have good ones in the vector and can have a one to one mapping without having to check for NaN's / having a separate boolean vector indicating if it is a good point etc.
00338                     std::cout << "kpts.size() : " << kpts.size() << std::endl;
00339                     std::cout << "idx_prototype.size() : " << idx_prototype.size() << std::endl;
00340                     assert((int)kpts.size() == idx_prototype.size());
00341                }
00342     */
00343 };
00344 
00345 
00346 template <typename PointSource, typename PointTarget>
00347 class PoseEstimator
00348 {
00349 public:
00350     PoseEstimator(int NRansac,
00351                   double maxidx, double maxidd);
00352     size_t estimate(const NDTFrame<PointSource> &f0, const NDTFrame<PointTarget> &f1);
00353     size_t estimate(const NDTFrame<PointSource> &f0, const NDTFrame<PointTarget> &f1, const std::vector<cv::DMatch>& matches);
00354 
00355     const std::vector<cv::DMatch>& getInliers() const
00356     {
00357         return inliers;
00358     }
00359 
00360     void matchFrames(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f2, std::vector<cv::DMatch>& fwd_matches);
00361 
00362     inline Eigen::Affine3f getTransform()
00363     {
00364         Eigen::Affine3f transl_transform = (Eigen::Affine3f)Eigen::Translation3f(trans[0], trans[1], trans[2]);
00365         Eigen::Affine3f rot_transform = (Eigen::Affine3f)Eigen::Matrix3f(rot.cast<float>());
00366         return transl_transform*rot_transform;
00367     }
00368 
00369     // all matches and inliers
00370     std::vector<cv::DMatch> matches; 
00371     std::vector<cv::DMatch> inliers; 
00372 
00373     int numRansac;
00374 
00376     bool windowed;
00377 
00379     double maxInlierXDist2, maxInlierDDist2;
00380 
00381     double maxDist;
00382     double minDist;
00383 
00385     cv::Ptr<cv::DescriptorMatcher> matcher;
00386     int wx /*< Width of matching window.*/, wy /*< Height of matching window*/;
00387 
00388     // transform
00389     Eigen::Matrix3d rot; 
00390     Eigen::Quaterniond quat;
00391     Eigen::Vector3d trans; 
00392 
00393     bool projectMatches;
00394 };
00395 
00396 
00397 } // namespace
00398 
00399 #include <ndt_feature_reg/impl/ndt_frame.hpp>
00400 
00401 #endif


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