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 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)
00109 {
00110
00111 cv::Mat pixels = cv::Mat(size.height * size.width,1, CV_64FC2);
00112
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);
00127 cv::undistortPoints(pixels, normpixels, _camMat, _dist);
00128
00129 _lookupTable = cv::Mat(normpixels.size(), CV_64FC3);
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
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
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
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
00198
00199
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
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 ¢er)
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
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
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
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
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
00335 }
00336
00337 }
00338 }
00339 }
00340
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
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
00366
00367 if(_lookupTable.empty())
00368 {
00369
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
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
00472 cov = tr.inverse()*cov*tr;
00473
00474 return size;
00475 }
00476 };
00477
00478
00479 };
00480
00481 #endif
00482