Subversion Repositories seema-scanner

Rev

Rev 242 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 242 Rev 245
Line 21... Line 21...
21 
#include <pcl/io/pcd_io.h>
 21 
#include <pcl/io/pcd_io.h>
22 
#include <pcl/features/normal_3d.h>
 22 
#include <pcl/features/normal_3d.h>
23 
#include <pcl/features/normal_3d_omp.h>
 23 
#include <pcl/features/normal_3d_omp.h>
24 
#include <pcl/common/transforms.h>
 24 
#include <pcl/common/transforms.h>
25 
 
 25 
 
- 
 
 26 
/* Convert everything to Debayered floating point frames */
- 
 
 27 
void debayerAndFloat(const std::vector<cv::Mat> &rawFrames, std::vector<cv::Mat> &frames){
- 
 
 28 
 
- 
 
 29 
    unsigned int nFrames = rawFrames.size();
- 
 
 30 
    frames.resize(nFrames);
- 
 
 31 
 
- 
 
 32 
    assert(rawFrames[0].type() == CV_8UC1);
- 
 
 33 
 
- 
 
 34 
    // Debayer and convert to float
- 
 
 35 
    for(unsigned int i=0; i<nFrames; i++){
- 
 
 36 
        cv::cvtColor(rawFrames[i], frames[i], CV_BayerBG2RGB);
- 
 
 37 
        frames[i].convertTo(frames[i], CV_32FC3, 1.0/255.0);
- 
 
 38 
    }
- 
 
 39 
 
- 
 
 40 
}
- 
 
 41 
 
- 
 
 42 
/* Merge exposures of HDR sequence */
- 
 
 43 
void mergeHDR(const std::vector<cv::Mat> &frames, const std::vector<float> &shutters, std::vector<cv::Mat> &hdrFrames){
- 
 
 44 
 
- 
 
 45 
    int nShutters = shutters.size();
- 
 
 46 
    unsigned int nFrames = frames.size();
- 
 
 47 
    unsigned int nHDRFrames = nFrames/nShutters;
- 
 
 48 
 
- 
 
 49 
    assert(nShutters * nHDRFrames == nFrames);
- 
 
 50 
 
- 
 
 51 
    int nRows = frames[0].rows;
- 
 
 52 
    int nCols = frames[0].cols;
- 
 
 53 
 
- 
 
 54 
    // Merge into HDR
- 
 
 55 
    std::vector<cv::Mat> outputFrames(nHDRFrames);
- 
 
 56 
 
- 
 
 57 
    float shutterMin = shutters[0];
- 
 
 58 
 
- 
 
 59 
    for(unsigned int i=0; i<nHDRFrames; i++){
- 
 
 60 
        outputFrames[i].create(nRows, nCols, CV_32FC3);
- 
 
 61 
        outputFrames[i].setTo(0.0);
- 
 
 62 
    }
- 
 
 63 
 
- 
 
 64 
    #pragma omp parallel for
- 
 
 65 
    for(unsigned int j=0; j<nShutters; j++){
- 
 
 66 
 
- 
 
 67 
        std::vector<cv::Mat> frameChannels;
- 
 
 68 
        cv::split(frames[j*nHDRFrames], frameChannels);
- 
 
 69 
 
- 
 
 70 
        cv::Mat mask = (frameChannels[0] < 0.99) & (frameChannels[1] < 0.99) & (frameChannels[2] < 0.99);
- 
 
 71 
 
- 
 
 72 
        for(unsigned int i=0; i<nHDRFrames; i++){
- 
 
 73 
            cv::Mat frameji = frames[j*nHDRFrames + i];
- 
 
 74 
 
- 
 
 75 
            cv::add((shutterMin/shutters[j]) * frameji, outputFrames[i], outputFrames[i], mask);
- 
 
 76 
 
- 
 
 77 
        }
- 
 
 78 
    }
- 
 
 79 
 
- 
 
 80 
    hdrFrames = outputFrames;
- 
 
 81 
}
26 
 
 82 
 
27 
void SMReconstructionWorker::reconstructPointCloud(const SMFrameSequence &frameSequence){
 83 
void SMReconstructionWorker::reconstructPointCloud(const SMFrameSequence &frameSequence){
28 
 
 84 
 
- 
 
 85 
    time.start();
- 
 
 86 
 
29 
    QSettings settings;
 87 
    QSettings settings;
30 
 
 88 
 
31 
    // Get current calibration
 89 
    // Get current calibration
32 
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 90 
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
33 
 
 91 
 
Line 53... Line 111...
53 
    else{
 111 
    else{
54 
        std::cerr << "SLScanWorker: invalid codec (Please set codec in preferences): " << codec.toStdString() << std::endl;
 112 
        std::cerr << "SLScanWorker: invalid codec (Please set codec in preferences): " << codec.toStdString() << std::endl;
55 
        return; // otherwise segfault TODO no default?
 113 
        return; // otherwise segfault TODO no default?
56 
    }
 114 
    }
57 
 
 115 
 
58 
    assert(frameSequence.frames0.size() == algorithm->getNPatterns());
 116 
    // Convert data to floating point and debayer
59 
    assert(frameSequence.frames1.size() == algorithm->getNPatterns());
 117 
    unsigned int nFrames = frameSequence.frames0.size();
60 
 
 118 
 
- 
 
 119 
    std::vector<cv::Mat> frames0, frames1;
- 
 
 120 
    debayerAndFloat(frameSequence.frames0, frames0);
- 
 
 121 
    debayerAndFloat(frameSequence.frames1, frames1);
- 
 
 122 
 
61 
    // Print OpenCV build information
 123 
    // If HDR sequence, merge frames
62 
    cv::setUseOptimized(true);
 124 
    if(frameSequence.shutters.size() > 1){
63 
    //std::cout << cv::getBuildInformation();
 125 
        mergeHDR(frames0, frameSequence.shutters, frames0);
- 
 
 126 
        mergeHDR(frames1, frameSequence.shutters, frames1);
- 
 
 127 
    }
64 
 
 128 
 
- 
 
 129 
    assert(frames0.size() == algorithm->getNPatterns());
65 
    time.start();
 130 
    assert(frames1.size() == algorithm->getNPatterns());
66 
 
 131 
 
67 
    // Get 3D Points
 132 
    // Get 3D Points
68 
    std::vector<cv::Point3f> Q;
 133 
    std::vector<cv::Point3f> Q;
69 
    std::vector<cv::Vec3b> color;
 134 
    std::vector<cv::Vec3f> color;
70 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
 135 
    algorithm->get3DPoints(calibration, frames0, frames1, Q, color);
71 
 
 136 
 
72 
    // Convert point cloud to PCL format
 137 
    // Convert point cloud to PCL format
73 
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 138 
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
74 
 
 139 
 
75 
    pointCloudPCL->width = Q.size();
 140 
    pointCloudPCL->width = Q.size();
Line 79... Line 144...
79 
    pointCloudPCL->points.resize(Q.size());
 144 
    pointCloudPCL->points.resize(Q.size());
80 
 
 145 
 
81 
    for(unsigned int i=0; i<Q.size(); i++){
 146 
    for(unsigned int i=0; i<Q.size(); i++){
82 
        pcl::PointXYZRGBNormal point;
 147 
        pcl::PointXYZRGBNormal point;
83 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
 148 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
84 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
 149 
        point.r = 255*color[i][0]; point.g = 255*color[i][1]; point.b = 255*color[i][2];
85 
        pointCloudPCL->points[i] = point;
 150 
        pointCloudPCL->points[i] = point;
86 
    }
 151 
    }
87 
 
 152 
 
88 
    // Transform point cloud to rotation axis coordinate system
 153 
    // Transform point cloud to rotation axis coordinate system
89 
    /*cv::Mat TRCV(3, 4, CV_32F);
 154 
    /*cv::Mat TRCV(3, 4, CV_32F);
Line 137... Line 202...
137 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 202 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
138 
    std::cout << "SMReconstructionWorker: " << smPointCloud.pointCloud->size() << " Points" << std::endl;
 203 
    std::cout << "SMReconstructionWorker: " << smPointCloud.pointCloud->size() << " Points" << std::endl;
139 
 
 204 
 
140 
}
 205 
}
141 
 
 206 
 
- 
 
 207 
 
142 
void SMReconstructionWorker::reconstructPointClouds(const std::vector<SMFrameSequence> &frameSequences){
 208 
void SMReconstructionWorker::reconstructPointClouds(const std::vector<SMFrameSequence> &frameSequences){
143 
 
 209 
 
144 
    // Process sequentially
 210 
    // Process sequentially
145 
    #pragma omp parallel for
 211 
    #pragma omp parallel for
146 
    for(unsigned int i=0; i<frameSequences.size(); i++){
 212 
    for(unsigned int i=0; i<frameSequences.size(); i++){