Guest User

depth.cpp

a guest
Mar 25th, 2016
188
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 10.44 KB | None | 0 0
  1. #include "depth.h"
  2. #include <opencv2/opencv.hpp>
  3. #include <math.h>
  4. #include <limits>
  5. #include <string>
  6. #include <boost/timer.hpp>
  7. #include <pcl/visualization/cloud_viewer.h>
  8.  
  9. using namespace tools;
  10.  
  11. Camera::Camera() {
  12.     this->principalPointX = 0.;
  13.     this->principalPointY = 0.;
  14.     this->focalLengthX = 1.;
  15.     this->focalLengthY = 1.;
  16. }
  17.  
  18. Camera::Camera(float principalPointX, float principalPointY, float focalLengthX, float focalLengthY) {
  19.     assert(focalLengthX != 0);
  20.     assert(focalLengthY != 0);
  21.    
  22.     this->principalPointX = principalPointX;
  23.     this->principalPointY = principalPointY;
  24.     this->focalLengthX = focalLengthX;
  25.     this->focalLengthY = focalLengthY;
  26. }
  27.  
  28. float Camera::projectZ(unsigned short depth, float factor) {
  29.     return (float) (depth/factor);
  30. }
  31.  
  32. template <typename T>
  33. float Camera::projectX(T x, unsigned short depth, float factor) {
  34.     return (((float) x) - this->principalPointX) * (((float) depth)/factor)/this->focalLengthX;
  35. }
  36.  
  37. template float Camera::projectX<int>(int, unsigned short, float);
  38. template float Camera::projectX<float>(float, unsigned short, float);
  39.  
  40. template <typename T>
  41. float Camera::projectY(T y, unsigned short depth, float factor) {
  42.     return (((float) y) - this->principalPointY) * (((float) depth)/factor)/this->focalLengthY;
  43. }
  44.  
  45. template float Camera::projectY<int>(int, unsigned short, float);
  46. template float Camera::projectY<float>(float, unsigned short, float);
  47.  
  48. int Camera::unprojectX(float x, float depth) {
  49.     return round(x*this->focalLengthX/depth + this->principalPointX);
  50. }
  51.  
  52. int Camera::unprojectY(float y, float depth) {
  53.     return round(y*this->focalLengthY/depth + this->principalPointY);
  54. }
  55.  
  56. pcl::PointCloud<pcl::PointXYZ>::Ptr Depth::pointCloudFromDepth(const cv::Mat &depth, Camera camera, float factor) {
  57.     assert(depth.channels() == 1);
  58.    
  59.     cv::Mat depthImage = depth.clone();
  60.     depth.convertTo(depthImage, CV_16UC1);
  61.    
  62.     pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
  63.     pointCloud->height = depth.rows;
  64.     pointCloud->width = depth.cols;
  65.     pointCloud->is_dense = true;
  66.     pointCloud->resize(pointCloud->height*pointCloud->width);
  67.    
  68.     for (int i = 0; i < depth.rows; ++i) {
  69.         for (int j = 0; j < depth.cols; ++j) {
  70.             pcl::PointXYZ point;
  71.            
  72.             // x,y,z coordinates are in meters, but depth is given in meters*1000.
  73.             point.z = camera.projectZ(depth.at<unsigned short>(i, j));
  74.             point.x = camera.projectX(j, depth.at<unsigned short>(i, j));
  75.             point.y = camera.projectY(i, depth.at<unsigned short>(i, j));
  76.            
  77.             // Note that row and column are switched here.
  78.             (*pointCloud)(j, i) = point;
  79.         }
  80.     }
  81.    
  82.     return pointCloud;
  83. }
  84.  
  85. pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Depth::pointCloudFromBGRAndDepth(const cv::Mat &image, const cv::Mat &depth, Camera camera, float factor) {
  86.     assert(depth.channels() == 1);
  87.     assert(image.channels() == 3);
  88.     assert(image.rows == depth.rows);
  89.     assert(image.cols == depth.cols);
  90.    
  91.     cv::Mat depthImage = depth.clone();
  92.     depth.convertTo(depthImage, CV_16UC1);
  93.    
  94.     cv::Mat colorImage = image.clone();
  95.     if (image.channels() == 3) {
  96.         image.convertTo(colorImage, CV_8UC3);
  97.     }
  98.    
  99.     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
  100.     pointCloud->height = depth.rows;
  101.     pointCloud->width = depth.cols;
  102.     pointCloud->is_dense = true;
  103.     pointCloud->resize(pointCloud->height*pointCloud->width);
  104.    
  105.     for (int i = 0; i < depth.rows; ++i) {
  106.         for (int j = 0; j < depth.cols; ++j) {
  107.             pcl::PointXYZRGBA point;
  108.            
  109.             // x,y,z coordinates are in meters, but depth is given in meters*1000.
  110.             point.z = camera.projectZ(depth.at<unsigned short>(i, j));
  111.             point.x = camera.projectX(j, depth.at<unsigned short>(i, j));
  112.             point.y = camera.projectY(i, depth.at<unsigned short>(i, j));
  113.            
  114.             point.r = colorImage.at<cv::Vec3b>(i, j)[2];
  115.             point.g = colorImage.at<cv::Vec3b>(i, j)[1];
  116.             point.b = colorImage.at<cv::Vec3b>(i, j)[0];
  117.            
  118.             // Note that row and column are switched here.
  119.             (*pointCloud)(j, i) = point;
  120.         }
  121.     }
  122.    
  123.     return pointCloud;
  124. }
  125.  
  126. pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Depth::unorderedPointCloudFromBGRAndDepth(const cv::Mat &image, const cv::Mat &depth, Camera camera, float factor) {
  127.     assert(depth.channels() == 1);
  128.     assert(image.channels() == 3);
  129.     assert(image.rows == depth.rows);
  130.     assert(image.cols == depth.cols);
  131.    
  132.     cv::Mat depthImage = depth.clone();
  133.     depth.convertTo(depthImage, CV_16UC1);
  134.    
  135.     cv::Mat colorImage = image.clone();
  136.     if (image.channels() == 3) {
  137.         image.convertTo(colorImage, CV_8UC3);
  138.     }
  139.    
  140.     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
  141.    
  142.     int index = 0;
  143.     for (int i = 0; i < depth.rows; ++i) {
  144.         for (int j = 0; j < depth.cols; ++j) {
  145.             if (depth.at<unsigned short>(i, j) > 0) {
  146.                 pcl::PointXYZRGBA point;
  147.  
  148.                 // x,y,z coordinates are in meters, but depth is given in meters*1000.
  149.                 point.z = camera.projectZ(depth.at<unsigned short>(i, j));
  150.                 point.x = camera.projectX(j, depth.at<unsigned short>(i, j));
  151.                 point.y = camera.projectY(i, depth.at<unsigned short>(i, j));
  152.  
  153.                 point.r = colorImage.at<cv::Vec3b>(i, j)[2];
  154.                 point.g = colorImage.at<cv::Vec3b>(i, j)[1];
  155.                 point.b = colorImage.at<cv::Vec3b>(i, j)[0];
  156.                 std::cout << point << std::endl;
  157.                 // Note that row and column are switched here.
  158.                 pointCloud->push_back(point);
  159.                 ++index;
  160.             }
  161.         }
  162.     }
  163.    
  164.     return pointCloud;
  165. }
  166.  
  167. template <typename T>
  168. cv::Mat Depth::drawNormalImage(typename pcl::PointCloud<T>::Ptr pointCloud, int* bgr) {
  169.     assert(pointCloud->height > 1);
  170.    
  171.     pcl::IntegralImageNormalEstimation<T, pcl::Normal> normalEstimator;
  172.     typename pcl::search::KdTree<T>::Ptr tree(new pcl::search::KdTree<T>);
  173.     pcl::PointCloud<pcl::Normal>::Ptr normalCloud(new pcl::PointCloud<pcl::Normal>);
  174.    
  175.     normalEstimator.setNormalEstimationMethod (normalEstimator.COVARIANCE_MATRIX);
  176.     normalEstimator.setMaxDepthChangeFactor(0.02f);
  177.     normalEstimator.setNormalSmoothingSize(10.0f);// @see http://docs.pointclouds.org/1.7.1/classpcl_1_1_integral_image_normal_estimation.html#a10da239414888271aeb02972f7420780
  178.     normalEstimator.setBorderPolicy(pcl::IntegralImageNormalEstimation<T, pcl::Normal>::BORDER_POLICY_MIRROR);
  179.     normalEstimator.setInputCloud(pointCloud);
  180.     normalEstimator.compute(*normalCloud);
  181.    
  182.     cv::Mat image(normalCloud->height, normalCloud->width, CV_8UC3);
  183.     for (int i = 0; i < image.rows; ++i) {
  184.         for (int j = 0; j < image.cols; ++j) {
  185.             pcl::Normal normal = (*normalCloud)(j, i);
  186.            
  187.             if (!pcl::isFinite<pcl::Normal>(normal)) {
  188.                 image.at<cv::Vec3b>(i, j)[0] = bgr[0];
  189.                 image.at<cv::Vec3b>(i, j)[1] = bgr[1];
  190.                 image.at<cv::Vec3b>(i, j)[2] = bgr[2];
  191.             }
  192.             else {
  193.                 double norm = sqrt(normal.normal_x*normal.normal_x + normal.normal_y*normal.normal_y + normal.normal_z*normal.normal_z);
  194.  
  195.                 image.at<cv::Vec3b>(i, j)[0] = (unsigned char) (normal.normal_x/norm*127) + 127;
  196.                 image.at<cv::Vec3b>(i, j)[1] = (unsigned char) (normal.normal_y/norm*127) + 127;
  197.                 image.at<cv::Vec3b>(i, j)[2] = (unsigned char) (normal.normal_z/norm*127) + 127;
  198.             }
  199.         }
  200.     }
  201.    
  202.     return image;
  203. }
  204.  
  205. template cv::Mat Depth::drawNormalImage<pcl::PointXYZ>(pcl::PointCloud<pcl::PointXYZ>::Ptr, int*);
  206. template cv::Mat Depth::drawNormalImage<pcl::PointXYZRGBA>(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr, int*);
  207.  
  208. template <typename T>
  209. cv::Mat Depth::drawNormalSegments(typename pcl::PointCloud<T>::Ptr pointCloud, int** labels) {
  210.     assert(pointCloud->height > 1);
  211.    
  212.     int rows = pointCloud->height;
  213.     int cols = pointCloud->width;
  214.     int maxLabel = 0;
  215.    
  216.     cv::Mat image(rows, cols, CV_8UC3);
  217.    
  218.     for (int i = 0; i < rows; ++i) {
  219.         for (int j = 0; j < cols; ++j) {
  220.             if (labels[i][j] > maxLabel) {
  221.                 maxLabel = labels[i][j];
  222.             }
  223.         }
  224.     }
  225.    
  226.     for (int label = 0; label < maxLabel; ++label) {
  227.         std::vector<int> indices;
  228.        
  229.         for (int i = 0; i < rows; ++i) {
  230.             for (int j = 0; j < cols; ++j) {
  231.                 if (labels[i][j] == label) {
  232.                     indices.push_back(i*cols + j);
  233.                 }
  234.             }
  235.         }
  236.        
  237.         Eigen::Vector4f superpixelCentroid;
  238.         Eigen::Matrix3f superpixelCovariance;
  239.         Eigen::Vector3f superpixelNormal;
  240.        
  241.         pcl::compute3DCentroid(*pointCloud, indices, superpixelCentroid);
  242.         pcl::computeCovarianceMatrix(*pointCloud, indices, superpixelCentroid, superpixelCovariance);
  243.         Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> superpixelEigenValues(superpixelCovariance);
  244.         superpixelNormal = superpixelEigenValues.eigenvectors().col(0);
  245.        
  246.         bool flipped = false;
  247.         for (int i = 0; i < rows; ++i) {
  248.             for (int j = 0; j < cols; ++j) {
  249.                 if (labels[i][j] == label) {
  250.                     if (flipped == false) {
  251.                         pcl::flipNormalTowardsViewpoint((*pointCloud)(j, i), 0, 0, 0, superpixelNormal(0), superpixelNormal(1), superpixelNormal(2));
  252.                         flipped = true;
  253.                     }
  254.                    
  255.                     image.at<cv::Vec3b>(i, j)[0] = (unsigned char) (superpixelNormal(0)/superpixelNormal.norm()*127) + 127;
  256.                     image.at<cv::Vec3b>(i, j)[1] = (unsigned char) (superpixelNormal(1)/superpixelNormal.norm()*127) + 127;
  257.                     image.at<cv::Vec3b>(i, j)[2] = (unsigned char) (superpixelNormal(2)/superpixelNormal.norm()*127) + 127;
  258.                 }
  259.             }
  260.         }
  261.     }
  262.    
  263.     return image;
  264. }
  265.  
  266. template cv::Mat Depth::drawNormalSegments<pcl::PointXYZ>(pcl::PointCloud<pcl::PointXYZ>::Ptr, int**);
  267. template cv::Mat Depth::drawNormalSegments<pcl::PointXYZRGBA>(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr, int**);
Add Comment
Please, Sign In to add comment