Subversion Repositories seema-scanner

#include "SMTriangulationWorker.h"

#include <QCoreApplication>

#include <QSettings>

#include <iostream>

#include <opencv2/opencv.hpp>

#include <pcl/filters/statistical_outlier_removal.h>

#include <pcl/io/pcd_io.h>

void SMTriangulationWorker::setup(){

    // Initialize triangulator with calibration

    calibration = new SMCalibrationParams;

    calibration->load("calibration.yml");

    triangulator = new Triangulator(*calibration, frameWidth, frameHeight);

    QSettings settings("SLStudio");

    writeToDisk = settings.value("writeToDisk/pointclouds",false).toBool();

}

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

    time.start();

    // Reconstruct point cloud

    cv::Mat pointCloud;

    cv::Mat empty;

    triangulator->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 << "Triangulator: " << time.elapsed() << "ms" << std::endl;

    if(writeToDisk){

        QString fileName = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmsszzz");

        fileName.append(".pcd");

        pcl::io::savePCDFileBinary(fileName.toStdString(), *pointCloudPCL);

    }

    //emit finished();

}

SMTriangulationWorker::~SMTriangulationWorker(){

    delete calibration;

    delete triangulator;

    std::cout<<"triangulatorWorker deleted\n"<<std::flush;

}