depth_camera.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 DEPTH_CAMERA
00036 #define DEPTH_CAMERA
00037 
00038 #include <cv.h>
00039 #include <pcl/point_cloud.h>
00040 #include <iostream>
00041 
00042 namespace lslgeneric
00043 {
00044 
00045 template <typename PointT>
00046 class DepthCamera
00047 {
00048 private:
00049     cv::Mat _camMat, _dist;
00050     cv::Mat _lookupTable;
00051 
00052     inline cv::Mat getCameraMatrix(double fx, double fy, double cx, double cy)
00053     {
00054         cv::Mat ret = cv::Mat::zeros(3,3, CV_64F);
00055         ret.at<double>(0,0) = fx;
00056         ret.at<double>(1,1) = fy;
00057         ret.at<double>(0,2) = cx;
00058         ret.at<double>(1,2) = cy;
00059         ret.at<double>(2,2) = 1.;
00060         return ret;
00061     }
00062 
00063     inline cv::Mat getDistVector(double d0, double d1, double d2, double d3, double d4)
00064     {
00065         cv::Mat ret = cv::Mat(5,1, CV_64F);
00066         ret.at<double>(0) = d0;
00067         ret.at<double>(1) = d1;
00068         ret.at<double>(2) = d2;
00069         ret.at<double>(3) = d3;
00070         ret.at<double>(4) = d4;
00071         return ret;
00072     }
00073 
00074 
00075 public:
00076 
00077     double fx, fy, cx, cy, ds, scale_;
00078     std::vector<double> dist;
00079     bool isFloatImg;
00080 
00081     DepthCamera() { }
00082     DepthCamera (double &_fx, double &_fy, double &_cx, double &_cy, std::vector<double> &_distances, double _ds, bool _isFloatImg = false):
00083         fx(_fx),fy(_fy),cx(_cx),cy(_cy),ds(_ds),isFloatImg(_isFloatImg)
00084     {
00085         dist = _distances;
00086         _camMat = getCameraMatrix(fx, fy, cx, cy);
00087         _dist = getDistVector(dist[0], dist[1], dist[2], dist[3], dist[4]);
00088     }
00089     DepthCamera (const DepthCamera &other)
00090     {
00091         fx = other.fx;
00092         fy = other.fy;
00093         cx = other.cx;
00094         cy = other.cy;
00095         ds = other.ds;
00096         dist = other.dist;
00097         isFloatImg = other.isFloatImg;
00098         _camMat = getCameraMatrix(fx, fy, cx, cy);
00099         _dist = getDistVector(dist[0], dist[1], dist[2], dist[3], dist[4]);
00100         _lookupTable = other._lookupTable;
00101     }
00102 
00103     inline void setLookupTable(cv::Mat lookup)
00104     {
00105         _lookupTable = lookup;
00106     }
00107 
00108     inline void setupDepthPointCloudLookUpTable(const cv::Size &size) //, const cv::Mat &camMat, const cv::Mat &distVec, const double &dsFactor)
00109     {
00110 
00111         cv::Mat pixels = cv::Mat(size.height * size.width,1, CV_64FC2);
00112         // Fill the tmp values simply with the image coordinates
00113         {
00114             cv::Mat_<cv::Vec2d> _I = pixels;
00115             size_t iter = 0;
00116             for (int y = 0; y < size.height; y++)
00117             {
00118                 for (int x = 0; x < size.width; x++)
00119                 {
00120                     _I(iter)[0] = x;
00121                     _I(iter)[1] = y;
00122                     iter++;
00123                 }
00124             }
00125         }
00126         cv::Mat normpixels = cv::Mat(pixels.size(), CV_64FC2); // normalized undistorted pixels
00127         cv::undistortPoints(pixels, normpixels, _camMat, _dist);
00128 
00129         _lookupTable = cv::Mat(normpixels.size(), CV_64FC3); // "normpixelsxyz"
00130         {
00131             cv::Mat_<cv::Vec2d> _J = normpixels;
00132             cv::Mat_<cv::Vec3d> _I = _lookupTable;
00133             size_t iter = 0;
00134             for (int y = 0; y < size.height; y++)
00135             {
00136                 for (int x = 0; x < size.width; x++)
00137                 {
00138                     _I(iter)[0] = _J(iter)[0]*ds;
00139                     _I(iter)[1] = _J(iter)[1]*ds;
00140                     _I(iter)[2] = ds;
00141                     iter++;
00142                 }
00143             }
00144         }
00145     }
00146 
00147     inline size_t convertDepthImageToPointCloud(const cv::Mat &depthImg, pcl::PointCloud<PointT> &pc)
00148     {
00149         if (depthImg.depth() != CV_16U && !(isFloatImg) )
00150         {
00151             std::cerr<<"wrong depth image format - expected raw 16bit data\n";
00152             return 0;
00153         }
00154         if (depthImg.depth() != CV_32F && isFloatImg )
00155         {
00156             std::cerr<<"wrong depth image format - expected 32 bit floats\n";
00157             return 0;
00158         }
00159         float nan = std::numeric_limits<float>::quiet_NaN();
00160 
00161         size_t width = depthImg.size().width;
00162         size_t height = depthImg.size().height;
00163         size_t size = width*height;
00164         //std::cout<<"depth image "<<width<<"x"<<height<<" = "<<size<<std::endl;
00165         if (pc.size() != size || pc.width != width || pc.height != height || pc.is_dense != true)
00166         {
00167             pc.resize(size);
00168             pc.is_dense = true;
00169             pc.width = width;
00170             pc.height = height;
00171         }
00172         if(_lookupTable.empty())
00173         {
00174             //std::cout<<"setting up lookup table\n";
00175             this->setupDepthPointCloudLookUpTable(depthImg.size());
00176         }
00177         cv::Mat_<cv::Vec3d> _I = _lookupTable;
00178         double depth;
00179         if(!isFloatImg)
00180         {
00181             const unsigned short* pd = depthImg.ptr<unsigned short>(0);
00182             for (size_t i = 0; i < size; i++)
00183             {
00184                 if (*pd == 0)
00185                 {
00186                     //pc[i] = PointT(nan,nan,nan);
00187                     pc[i].x = nan;
00188                     pc[i].y = nan;
00189                     pc[i].z = nan;
00190                 }
00191                 else
00192                 {
00193                     depth = *pd;
00194                     pc[i].x = depth * _I(i)[0];
00195                     pc[i].y = depth * _I(i)[1];
00196                     pc[i].z = depth * _I(i)[2];
00197 //                            depth * _I(i)[1],
00198 //                            depth * _I(i)[2]);
00199                     //    std::cout<<"depth "<<depth<<" -> "<<pc[i].x<<" "<<pc[i].y<<" "<<pc[i].z<<std::endl;
00200                 }
00201                 pd++;
00202             }
00203         }
00204         else
00205         {
00206             const float* pd = depthImg.ptr<float>(0);
00207             for (size_t i = 0; i < size; i++)
00208             {
00209                 if (*pd == 0)
00210                 {
00211                     pc[i] = PointT(nan,nan,nan);
00212                 }
00213                 else
00214                 {
00215                     depth = *pd;
00216                     pc[i] = PointT(depth * _I(i)[0],
00217                                    depth * _I(i)[1],
00218                                    depth * _I(i)[2]);
00219                     //    std::cout<<"depth "<<depth<<" -> "<<pc[i].x<<" "<<pc[i].y<<" "<<pc[i].z<<std::endl;
00220                 }
00221                 pd++;
00222             }
00223         }
00224         return size;
00225     }
00226 
00227     inline size_t computePointsAtIndex(const cv::Mat &depthImg, cv::KeyPoint &keyPointCenter, size_t &support_size, pcl::PointCloud<PointT> &pc, PointT &center)
00228     {
00229         if (depthImg.depth() != CV_16U && !(isFloatImg) )
00230         {
00231             std::cerr<<"wrong depth image format - expected raw 16bit data\n";
00232             return 0;
00233         }
00234         if (depthImg.depth() != CV_32F && isFloatImg )
00235         {
00236             std::cerr<<"wrong depth image format - expected 32 bit floats\n";
00237             return 0;
00238         }
00239         float nan = std::numeric_limits<float>::quiet_NaN();
00240 
00241         size_t halfSize = support_size/2;
00242         int halfSizeI = (int)halfSize;
00243 
00244         size_t width = 2*halfSize+1;
00245         size_t height = 2*halfSize+1;
00246         size_t size = width*height;
00247         //std::cout<<"keypoint region "<<width<<"x"<<height<<" = "<<size<<std::endl;
00248         if (pc.size() != size || pc.width != width || pc.height != height || pc.is_dense != true)
00249         {
00250             pc.resize(size);
00251             pc.is_dense = true;
00252             pc.width = width;
00253             pc.height = height;
00254         }
00255 
00256         if(_lookupTable.empty())
00257         {
00258             //std::cout<<"setting up lookup table\n";
00259             this->setupDepthPointCloudLookUpTable(depthImg.size());
00260         }
00261 
00262         cv::Mat_<cv::Vec3d> _I = _lookupTable;
00263 
00264         int uKey = static_cast<int>(keyPointCenter.pt.x+0.5);
00265         int vKey = static_cast<int>(keyPointCenter.pt.y+0.5);
00266         //int indexKey = vKey * depthImg.size().width + uKey;
00267 
00268         int index, index_cloud, u, v;
00269         double depth;
00270         center = PointT(nan,nan,nan);
00271 
00272 
00273         if(!isFloatImg)
00274         {
00275             const unsigned short* pd = depthImg.ptr<unsigned short>(0);
00276             const unsigned short* pd_here;
00277             for (int i = -halfSizeI; i < halfSizeI+1; i++)
00278             {
00279                 for (int j = -halfSizeI; j < halfSizeI+1; j++)
00280                 {
00281                     v = vKey+j;
00282                     u = uKey+i;
00283                     if( u < 0 || v < 0 || u >= depthImg.size().width || v >= depthImg.size().height)
00284                     {
00285                         continue;
00286                     }
00287                     index = v * depthImg.size().width + u;
00288                     index_cloud = (j+halfSizeI) * width + (i+halfSizeI);
00289                     pd_here = pd+index;
00290                     if (*pd_here == 0)
00291                     {
00292                         pc[index_cloud] = PointT(nan,nan,nan);
00293                     }
00294                     else
00295                     {
00296                         depth = *pd_here;
00297                         pc[index_cloud] = PointT(depth * _I(index)[0],
00298                                                  depth * _I(index)[1],
00299                                                  depth * _I(index)[2]);
00300                         //            std::cout<<"depth "<<depth<<" -> "<<pc[index_cloud].x<<" "<<pc[index_cloud].y<<" "<<pc[index_cloud].z<<std::endl;
00301                     }
00302                 }
00303             }
00304         }
00305         else
00306         {
00307             const float* pd = depthImg.ptr<float>(0);
00308             const float* pd_here;
00309             for (int i = -halfSizeI; i < halfSizeI+1; i++)
00310             {
00311                 for (int j = -halfSizeI; j < halfSizeI+1; j++)
00312                 {
00313                     v = vKey+j;
00314                     u = uKey+i;
00315                     if( u < 0 || v < 0 || u >= depthImg.size().width || v >= depthImg.size().height)
00316                     {
00317                         continue;
00318                     }
00319                     index = v * depthImg.size().width + u;
00320                     index_cloud = (j+halfSizeI) * width + (i+halfSizeI);
00321                     pd_here = pd+index;
00322 
00323                     if (*pd_here == 0)
00324                     {
00325                         pc[index_cloud] = PointT(nan,nan,nan);
00326                     }
00327                     else
00328                     {
00329 
00330                         depth = *pd_here;
00331                         pc[index_cloud] = PointT(depth * _I(index)[0],
00332                                                  depth * _I(index)[1],
00333                                                  depth * _I(index)[2]);
00334                         //            std::cout<<"depth "<<depth<<" -> "<<pc[index_cloud].x<<" "<<pc[index_cloud].y<<" "<<pc[index_cloud].z<<std::endl;
00335                     }
00336 
00337                 }
00338             }
00339         }
00340         // Get the center (i == 0 && j = 0)
00341         center = pc[(halfSizeI) * width + (halfSizeI)];
00342         return size;
00343     }
00344 
00345     inline size_t computeParamsAtIndex(const cv::Mat &depthImg, cv::KeyPoint &keyPointCenter, size_t &support_size, Eigen::Vector3d &mean, Eigen::Matrix3d &cov)
00346     {
00347         if (depthImg.depth() != CV_16U && !(isFloatImg) )
00348         {
00349             std::cerr<<"wrong depth image format - expected raw 16bit data\n";
00350             return 0;
00351         }
00352         if (depthImg.depth() != CV_32F && isFloatImg )
00353         {
00354             std::cerr<<"wrong depth image format - expected 32 bit floats\n";
00355             return 0;
00356         }
00357         //float nan = std::numeric_limits<float>::quiet_NaN(); //?
00358 
00359         size_t halfSize = support_size/2;
00360         int halfSizeI = (int)halfSize;
00361 
00362         size_t width = 2*halfSize+1;
00363         size_t height = 2*halfSize+1;
00364         size_t size = width*height;
00365         //std::cout<<"keypoint region "<<width<<"x"<<height<<" = "<<size<<std::endl;
00366 
00367         if(_lookupTable.empty())
00368         {
00369             //std::cout<<"setting up lookup table\n";
00370             this->setupDepthPointCloudLookUpTable(depthImg.size());
00371         }
00372         cv::Mat_<cv::Vec3d> _I = _lookupTable;
00373 
00374         int uKey = static_cast<int>(keyPointCenter.pt.x+0.5);
00375         int vKey = static_cast<int>(keyPointCenter.pt.y+0.5);
00376         //int indexKey = vKey * depthImg.size().width + uKey;
00377 
00378         int index, u, v;
00379         double depth = 0;
00380 
00381         double depth_mean = 0, depth_var = 0;
00382 
00383         const unsigned short* pd = depthImg.ptr<unsigned short>(0);
00384         const unsigned short* pd_here;
00385         const float* pd_float = depthImg.ptr<float>(0);
00386         const float* pd_here_float;
00387 
00388         double depthsize = support_size*support_size;
00389 
00390         for (int q = 0; q<2; q++)
00391         {
00392             for (int i = -halfSizeI; i < halfSizeI+1; i++)
00393             {
00394                 for (int j = -halfSizeI; j < halfSizeI+1; j++)
00395                 {
00396                     v = vKey+j;
00397                     u = uKey+i;
00398                     if( u < 0 || v < 0 || u >= depthImg.size().width || v >= depthImg.size().height)
00399                     {
00400                         continue;
00401                     }
00402                     index = v * depthImg.size().width + u;
00403                     pd_here = pd+index;
00404                     pd_here_float = pd_float+index;
00405 
00406                     if(!isFloatImg)
00407                     {
00408                         if (*pd_here != 0)
00409                         {
00410                             depth = *pd_here;
00411                         }
00412                     }
00413                     else
00414                     {
00415                         if (*pd_here_float != 0)
00416                         {
00417                             depth = *pd_here_float;
00418                         }
00419                     }
00420                     if(q==0)
00421                     {
00422                         depth_mean += depth/depthsize;
00423                     }
00424                     else
00425                     {
00426                         depth_var += pow((depth-depth_mean),2)/depthsize;
00427                     }
00428                 }
00429             }
00430         }
00431         double var = 0.001;
00432 
00433         index = vKey * depthImg.size().width + uKey;
00434         mean<<depth_mean*_I(index)[0],depth_mean*_I(index)[1],depth_mean*_I(index)[2];
00435 
00436         double x1 = depth_mean*_I(index)[0];
00437         double y1 = depth_mean*_I(index)[1];
00438 
00439         v = vKey+halfSize;
00440         u = uKey+halfSize;
00441         if( !(u < 0 || v < 0 || u >= depthImg.size().width || v >= depthImg.size().height) )
00442         {
00443             index = v * depthImg.size().width + u;
00444             pd_here = pd+index;
00445             pd_here_float = pd_float+index;
00446             if(!isFloatImg)
00447             {
00448                 if (*pd_here != 0)
00449                 {
00450                     depth = *pd_here;
00451                 }
00452             }
00453             else
00454             {
00455                 if (*pd_here_float != 0)
00456                 {
00457                     depth = *pd_here_float;
00458                 }
00459             }
00460             double x2 = depth*_I(index)[0];
00461             double y2 = depth*_I(index)[1];
00462             var = sqrt(pow(x1-x2,2)+pow(y1-y2,2))/4;
00463         }
00464 
00465         cov<<var,0,0,0,var,0,0,0,depth_var*ds*ds;
00466 
00467         Eigen::Vector3d eZ (0,0,1);
00468         double alpha = acos(eZ.dot(mean)/mean.norm());
00469         Eigen::AngleAxis<double> tr (-alpha, mean);
00470 
00471         //orient along beam direction
00472         cov = tr.inverse()*cov*tr;
00473 
00474         return size;
00475     }
00476 };
00477 
00478 
00479 };
00480 
00481 #endif
00482 


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:18:54