Subversion Repositories seema-scanner

#include "SMTriangulator.h"

#include <QCoreApplication>
#include <QSettings>

#include <iostream>
#include <opencv2/opencv.hpp>

#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>


SMTriangulator::SMTriangulator(){

    // Precompute lens correction maps
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1, lensMap2);

    //cv::Mat map1, map2;
    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
    //cv::imwrite("map1.png", map1);
    //cv::imwrite("map2.png", map2);
}

void SMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){

    time.start();

    // Reconstruct point cloud
    cv::Mat pointCloud;
    cv::Mat empty;
    triangulate(up, vp, mask, shading, pointCloud);

    std::vector<cv::Mat> xyz;
    cv::split(pointCloud, xyz);
//    emit imshow("x", xyz[0], 1400, 100);
//    emit imshow("y", xyz[1], 1400, 450);
//    emit imshow("z", xyz[2], 1400, 800);

    // Convert point cloud to PCL format
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);

    // Interprete as organized point cloud
    pointCloudPCL->width = pointCloud.cols;
    pointCloudPCL->height = pointCloud.rows;
    pointCloudPCL->is_dense = false;

    pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);

    //    for(int col=0; col<pointCloud.cols; col++){
    //        for(int row=0; row<pointCloud.rows; row++){
    //            const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
    //            unsigned char shade = shading.at<unsigned char>(row,col);
    //            pcl::PointXYZRGBRGB point;
    //            point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
    //            point.r = shade; point.g = shade; point.b = shade;
    //            pointCloudPCL->at(col, row) = point;
    //        }
    //    }

    // stack xyz data
    std::vector<cv::Mat> pointCloudChannels;
    pointCloudChannels.push_back(xyz[0]);
    pointCloudChannels.push_back(xyz[1]);
    pointCloudChannels.push_back(xyz[2]);

    // 4 byte padding
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));

    // triple uchar color information
    std::vector<cv::Mat> rgb;
    rgb.push_back(shading);
    rgb.push_back(shading);
    rgb.push_back(shading);
    rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));

    cv::Mat rgb8UC4;
    cv::merge(rgb, rgb8UC4);

    cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);

    pointCloudChannels.push_back(rgb32F);

    // 12 bytes padding
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));

    // merge channels
    cv::Mat pointCloudPadded;
    cv::merge(pointCloudChannels, pointCloudPadded);

    // memcpy everything
    memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));

//    // filtering
//    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> filter;
//    filter.setMeanK(5);
//    filter.setStddevMulThresh(1.0);
//    filter.setInputCloud(pointCloudPCL);
//    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
//    filter.filter(*pointCloudFiltered);

    // Emit result
    emit newPointCloud(pointCloudPCL);

    std::cout << "SMTriangulator: " << time.elapsed() << "ms" << std::endl;

    //emit finished();
}

void SMTriangulator::triangulate(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading, cv::Mat &pointCloud){

    // Undistort up, mask and shading
    if(!up.empty()){
        cv::Mat upUndistort;
        cv::remap(up, upUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
        up = upUndistort;
    }
    if(!vp.empty()){
        cv::Mat vpUndistort;
        cv::remap(vp, vpUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
        vp = vpUndistort;
    }

    cv::Mat maskUndistort, shadingUndistort;
    cv::remap(mask, maskUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
    cv::remap(shading, shadingUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
    mask = maskUndistort;
    shading = shadingUndistort;

    // Triangulate
    cv::Mat xyz;
    if(!up.empty() && vp.empty())
        triangulateFromUp(up, xyz);
    else if(!vp.empty() && up.empty())
        triangulateFromVp(vp, xyz);
    else if(!up.empty() && !vp.empty())
        triangulateFromUpVp(up, vp, xyz);

    // Merge and mask
    pointCloud = cv::Mat(up.size(), CV_32FC3, cv::Scalar(NAN, NAN, NAN));
    xyz.copyTo(pointCloud, mask);

}

void SMTriangulator::triangulateFromUp(cv::Mat &up, cv::Mat &xyz){

    // Solve for xyzw using determinant tensor
    cv::Mat C = determinantTensor;
    std::vector<cv::Mat> xyzw(4);
    for(unsigned int i=0; i<4; i++){
        xyzw[i].create(up.size(), CV_32F);
        xyzw[i] = C.at<float>(cv::Vec4i(i,0,1,0)) - C.at<float>(cv::Vec4i(i,2,1,0))*uc - C.at<float>(cv::Vec4i(i,0,2,0))*vc -
                C.at<float>(cv::Vec4i(i,0,1,2))*up + C.at<float>(cv::Vec4i(i,2,1,2))*up.mul(uc) + C.at<float>(cv::Vec4i(i,0,2,2))*up.mul(vc);
    }

    // Convert to non homogenous coordinates
    for(unsigned int i=0; i<3; i++)
        xyzw[i] /= xyzw[3];

    // Merge and mask
    cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);

}

void SMTriangulator::triangulateFromVp(cv::Mat &vp, cv::Mat &xyz){

    // Solve for xyzw using determinant tensor
    cv::Mat C = determinantTensor;
    std::vector<cv::Mat> xyzw(4);
    for(unsigned int i=0; i<4; i++){
        xyzw[i].create(vp.size(), CV_32F);
        xyzw[i] = C.at<float>(cv::Vec4i(i,0,1,1)) - C.at<float>(cv::Vec4i(i,2,1,1))*uc - C.at<float>(cv::Vec4i(i,0,2,1))*vc -
                C.at<float>(cv::Vec4i(i,0,1,2))*vp + C.at<float>(cv::Vec4i(i,2,1,2))*vp.mul(uc) + C.at<float>(cv::Vec4i(i,0,2,2))*vp.mul(vc);
    }

    // Convert to non homogenous coordinates
    for(unsigned int i=0; i<3; i++)
        xyzw[i] /= xyzw[3];

    // Merge and mask
    cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);

}

void SMTriangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){

//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
//    int N = up.rows * up.cols;

//    cv::Mat projPointsCam(2, N, CV_32F);
//    uc.reshape(0,1).copyTo(projPointsCam.row(0));
//    vc.reshape(0,1).copyTo(projPointsCam.row(1));

//    cv::Mat projPointsProj(2, N, CV_32F);
//    up.reshape(0,1).copyTo(projPointsProj.row(0));
//    vp.reshape(0,1).copyTo(projPointsProj.row(1));

//    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
//    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));

//    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
//    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
//    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
//    Pp = cv::Mat(calibration.Kp) * temp;

//    cv::Mat xyzw;
//    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);

//    xyz.create(3, N, CV_32F);
//    for(int i=0; i<N; i++){
//        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
//        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
//    }

//    xyz = xyz.t();
//    xyz = xyz.reshape(3, up.rows);
}

SMTriangulator::~SMTriangulator(){

    std::cout << "SMTriangulator deleted.\n" << std::flush;
}