depth.cpp

#include "depth.h"
#include <opencv2/opencv.hpp>
#include <math.h>
#include <limits>
#include <string>
#include <boost/timer.hpp>
#include <pcl/visualization/cloud_viewer.h>

using namespace tools;

Camera::Camera() {
    this->principalPointX = 0.;
    this->principalPointY = 0.;
    this->focalLengthX = 1.;
    this->focalLengthY = 1.;
}

Camera::Camera(float principalPointX, float principalPointY, float focalLengthX, float focalLengthY) {
    assert(focalLengthX != 0);
    assert(focalLengthY != 0);

    this->principalPointX = principalPointX;
    this->principalPointY = principalPointY;
    this->focalLengthX = focalLengthX;
    this->focalLengthY = focalLengthY;
}

float Camera::projectZ(unsigned short depth, float factor) {
    return (float) (depth/factor);
}

template <typename T>
float Camera::projectX(T x, unsigned short depth, float factor) {
    return (((float) x) - this->principalPointX) * (((float) depth)/factor)/this->focalLengthX;
}

template float Camera::projectX<int>(int, unsigned short, float);
template float Camera::projectX<float>(float, unsigned short, float);

template <typename T>
float Camera::projectY(T y, unsigned short depth, float factor) {
    return (((float) y) - this->principalPointY) * (((float) depth)/factor)/this->focalLengthY;
}

template float Camera::projectY<int>(int, unsigned short, float);
template float Camera::projectY<float>(float, unsigned short, float);

int Camera::unprojectX(float x, float depth) {
    return round(x*this->focalLengthX/depth + this->principalPointX);
}

int Camera::unprojectY(float y, float depth) {
    return round(y*this->focalLengthY/depth + this->principalPointY);
}

pcl::PointCloud<pcl::PointXYZ>::Ptr Depth::pointCloudFromDepth(const cv::Mat &depth, Camera camera, float factor) {
    assert(depth.channels() == 1);

    cv::Mat depthImage = depth.clone();
    depth.convertTo(depthImage, CV_16UC1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pointCloud->height = depth.rows;
    pointCloud->width = depth.cols;
    pointCloud->is_dense = true;
    pointCloud->resize(pointCloud->height*pointCloud->width);

    for (int i = 0; i < depth.rows; ++i) {
        for (int j = 0; j < depth.cols; ++j) {
            pcl::PointXYZ point;

            // x,y,z coordinates are in meters, but depth is given in meters*1000.
            point.z = camera.projectZ(depth.at<unsigned short>(i, j));
            point.x = camera.projectX(j, depth.at<unsigned short>(i, j));
            point.y = camera.projectY(i, depth.at<unsigned short>(i, j));

            // Note that row and column are switched here.
            (*pointCloud)(j, i) = point;
        }
    }

    return pointCloud;
}

pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Depth::pointCloudFromBGRAndDepth(const cv::Mat &image, const cv::Mat &depth, Camera camera, float factor) {
    assert(depth.channels() == 1);
    assert(image.channels() == 3);
    assert(image.rows == depth.rows);
    assert(image.cols == depth.cols);

    cv::Mat depthImage = depth.clone();
    depth.convertTo(depthImage, CV_16UC1);

    cv::Mat colorImage = image.clone();
    if (image.channels() == 3) {
        image.convertTo(colorImage, CV_8UC3);
    }

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pointCloud->height = depth.rows;
    pointCloud->width = depth.cols;
    pointCloud->is_dense = true;
    pointCloud->resize(pointCloud->height*pointCloud->width);

    for (int i = 0; i < depth.rows; ++i) {
        for (int j = 0; j < depth.cols; ++j) {
            pcl::PointXYZRGBA point;

            // x,y,z coordinates are in meters, but depth is given in meters*1000.
            point.z = camera.projectZ(depth.at<unsigned short>(i, j));
            point.x = camera.projectX(j, depth.at<unsigned short>(i, j));
            point.y = camera.projectY(i, depth.at<unsigned short>(i, j));

            point.r = colorImage.at<cv::Vec3b>(i, j)[2];
            point.g = colorImage.at<cv::Vec3b>(i, j)[1];
            point.b = colorImage.at<cv::Vec3b>(i, j)[0];

            // Note that row and column are switched here.
            (*pointCloud)(j, i) = point;
        }
    }

    return pointCloud;
}

pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Depth::unorderedPointCloudFromBGRAndDepth(const cv::Mat &image, const cv::Mat &depth, Camera camera, float factor) {
    assert(depth.channels() == 1);
    assert(image.channels() == 3);
    assert(image.rows == depth.rows);
    assert(image.cols == depth.cols);

    cv::Mat depthImage = depth.clone();
    depth.convertTo(depthImage, CV_16UC1);

    cv::Mat colorImage = image.clone();
    if (image.channels() == 3) {
        image.convertTo(colorImage, CV_8UC3);
    }

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>);

    int index = 0;
    for (int i = 0; i < depth.rows; ++i) {
        for (int j = 0; j < depth.cols; ++j) {
            if (depth.at<unsigned short>(i, j) > 0) {
                pcl::PointXYZRGBA point;

                // x,y,z coordinates are in meters, but depth is given in meters*1000.
                point.z = camera.projectZ(depth.at<unsigned short>(i, j));
                point.x = camera.projectX(j, depth.at<unsigned short>(i, j));
                point.y = camera.projectY(i, depth.at<unsigned short>(i, j));

                point.r = colorImage.at<cv::Vec3b>(i, j)[2];
                point.g = colorImage.at<cv::Vec3b>(i, j)[1];
                point.b = colorImage.at<cv::Vec3b>(i, j)[0];
                std::cout << point << std::endl;
                // Note that row and column are switched here.
                pointCloud->push_back(point);
                ++index;
            }
        }
    }

    return pointCloud;
}

template <typename T>
cv::Mat Depth::drawNormalImage(typename pcl::PointCloud<T>::Ptr pointCloud, int* bgr) {
    assert(pointCloud->height > 1);

    pcl::IntegralImageNormalEstimation<T, pcl::Normal> normalEstimator;
    typename pcl::search::KdTree<T>::Ptr tree(new pcl::search::KdTree<T>);
    pcl::PointCloud<pcl::Normal>::Ptr normalCloud(new pcl::PointCloud<pcl::Normal>);

    normalEstimator.setNormalEstimationMethod (normalEstimator.COVARIANCE_MATRIX);
    normalEstimator.setMaxDepthChangeFactor(0.02f);
    normalEstimator.setNormalSmoothingSize(10.0f);// @see http://docs.pointclouds.org/1.7.1/classpcl_1_1_integral_image_normal_estimation.html#a10da239414888271aeb02972f7420780
    normalEstimator.setBorderPolicy(pcl::IntegralImageNormalEstimation<T, pcl::Normal>::BORDER_POLICY_MIRROR);
    normalEstimator.setInputCloud(pointCloud);
    normalEstimator.compute(*normalCloud);

    cv::Mat image(normalCloud->height, normalCloud->width, CV_8UC3);
    for (int i = 0; i < image.rows; ++i) {
        for (int j = 0; j < image.cols; ++j) {
            pcl::Normal normal = (*normalCloud)(j, i);

            if (!pcl::isFinite<pcl::Normal>(normal)) {
                image.at<cv::Vec3b>(i, j)[0] = bgr[0];
                image.at<cv::Vec3b>(i, j)[1] = bgr[1];
                image.at<cv::Vec3b>(i, j)[2] = bgr[2];
            }
            else {
                double norm = sqrt(normal.normal_x*normal.normal_x + normal.normal_y*normal.normal_y + normal.normal_z*normal.normal_z);

                image.at<cv::Vec3b>(i, j)[0] = (unsigned char) (normal.normal_x/norm*127) + 127;
                image.at<cv::Vec3b>(i, j)[1] = (unsigned char) (normal.normal_y/norm*127) + 127;
                image.at<cv::Vec3b>(i, j)[2] = (unsigned char) (normal.normal_z/norm*127) + 127;
            }
        }
    }

    return image;
}

template cv::Mat Depth::drawNormalImage<pcl::PointXYZ>(pcl::PointCloud<pcl::PointXYZ>::Ptr, int*);
template cv::Mat Depth::drawNormalImage<pcl::PointXYZRGBA>(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr, int*);

template <typename T>
cv::Mat Depth::drawNormalSegments(typename pcl::PointCloud<T>::Ptr pointCloud, int** labels) {
    assert(pointCloud->height > 1);

    int rows = pointCloud->height;
    int cols = pointCloud->width;
    int maxLabel = 0;

    cv::Mat image(rows, cols, CV_8UC3);

    for (int i = 0; i < rows; ++i) {
        for (int j = 0; j < cols; ++j) {
            if (labels[i][j] > maxLabel) {
                maxLabel = labels[i][j];
            }
        }
    }

    for (int label = 0; label < maxLabel; ++label) {
        std::vector<int> indices;

        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                if (labels[i][j] == label) {
                    indices.push_back(i*cols + j);
                }
            }
        }

        Eigen::Vector4f superpixelCentroid;
        Eigen::Matrix3f superpixelCovariance;
        Eigen::Vector3f superpixelNormal;

        pcl::compute3DCentroid(*pointCloud, indices, superpixelCentroid);
        pcl::computeCovarianceMatrix(*pointCloud, indices, superpixelCentroid, superpixelCovariance);
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> superpixelEigenValues(superpixelCovariance);
        superpixelNormal = superpixelEigenValues.eigenvectors().col(0);

        bool flipped = false;
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                if (labels[i][j] == label) {
                    if (flipped == false) {
                        pcl::flipNormalTowardsViewpoint((*pointCloud)(j, i), 0, 0, 0, superpixelNormal(0), superpixelNormal(1), superpixelNormal(2));
                        flipped = true;
                    }

                    image.at<cv::Vec3b>(i, j)[0] = (unsigned char) (superpixelNormal(0)/superpixelNormal.norm()*127) + 127;
                    image.at<cv::Vec3b>(i, j)[1] = (unsigned char) (superpixelNormal(1)/superpixelNormal.norm()*127) + 127;
                    image.at<cv::Vec3b>(i, j)[2] = (unsigned char) (superpixelNormal(2)/superpixelNormal.norm()*127) + 127;
                }
            }
        }
    }

    return image;
}

template cv::Mat Depth::drawNormalSegments<pcl::PointXYZ>(pcl::PointCloud<pcl::PointXYZ>::Ptr, int**);
template cv::Mat Depth::drawNormalSegments<pcl::PointXYZRGBA>(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr, int**);