Subversion Repositories seema-scanner

Rev

Rev 128 | Rev 162 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
27 jakw 1 
#include "SMReconstructionWorker.h"
9 jakw 2 
 
41 jakw 3 
#include "AlgorithmGrayCode.h"
99 jakw 4 
#include "AlgorithmGrayCodeHorzVert.h"
128 jakw 5 
#include "AlgorithmPhaseShiftTwoFreq.h"
6 
#include "AlgorithmPhaseShiftThreeFreq.h"
123 jakw 7 
#include "AlgorithmLineShift.h"
27 jakw 8 
 
9 jakw 9 
#include <QCoreApplication>
10 
#include <QSettings>
11 
 
12 
#include <iostream>
13 
#include <opencv2/opencv.hpp>
14 
 
42 jakw 15 
#include "cvtools.h"
16 
 
9 jakw 17 
#include <pcl/filters/statistical_outlier_removal.h>
18 
#include <pcl/io/pcd_io.h>
44 jakw 19 
#include <pcl/features/normal_3d.h>
9 jakw 20 
 
21 
 
27 jakw 22 
void SMReconstructionWorker::setup(){
9 jakw 23 
 
137 jakw 24 
 
25 
}
26 
 
27 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
28 
 
27 jakw 29 
    QSettings settings;
30 
 
31 
    // Get current calibration
33 jakw 32 
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
27 jakw 33 
 
41 jakw 34 
    // Create Algorithm
137 jakw 35 
    QString codec = frameSequence.codec;
36 jakw 36 
    int resX = settings.value("projector/resX").toInt();
37 
    int resY = settings.value("projector/resY").toInt();
137 jakw 38 
 
71 jakw 39 
    if(codec == "GrayCode")
40 
        algorithm = new AlgorithmGrayCode(resX, resY);
107 jakw 41 
    else if(codec == "GrayCodeHorzVert")
99 jakw 42 
        algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);
128 jakw 43 
    else if(codec == "PhaseShiftTwoFreq")
44 
        algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);
45 
    else if(codec == "PhaseShiftThreeFreq")
46 
        algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);
123 jakw 47 
    else if(codec == "LineShift")
48 
        algorithm = new AlgorithmLineShift(resX, resY);
27 jakw 49 
    else
50 
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
51 
 
9 jakw 52 
    time.start();
53 
 
42 jakw 54 
    // Get 3D Points
36 jakw 55 
    std::vector<cv::Point3f> Q;
42 jakw 56 
    std::vector<cv::Vec3b> color;
103 jakw 57 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
9 jakw 58 
 
59 
    // Convert point cloud to PCL format
128 jakw 60 
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
9 jakw 61 
 
36 jakw 62 
    pointCloudPCL->width = Q.size();
63 
    pointCloudPCL->height = 1;
128 jakw 64 
    pointCloudPCL->is_dense = true;
9 jakw 65 
 
36 jakw 66 
    pointCloudPCL->points.resize(Q.size());
9 jakw 67 
 
75 jakw 68 
    for(unsigned int i=0; i<Q.size(); i++){
128 jakw 69 
        pcl::PointXYZRGBNormal point;
36 jakw 70 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
42 jakw 71 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
36 jakw 72 
        pointCloudPCL->points[i] = point;
27 jakw 73 
    }
9 jakw 74 
 
128 jakw 75 
    // Estimate surface normals
76 
    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
77 
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
78 
    pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
79 
    //ne.setKSearch(10);
80 
    ne.setRadiusSearch(0.5);
81 
    ne.setViewPoint(0.0, 0.0, 0.0);
82 
    ne.setInputCloud(pointCloudPCLCopy);
83 
    ne.compute(*pointCloudPCL);
44 jakw 84 
 
85 
    // Assemble SMPointCloud data structure
42 jakw 86 
    SMPointCloud smPointCloud;
45 jakw 87 
    smPointCloud.id = frameSequence.id;
42 jakw 88 
    smPointCloud.pointCloud = pointCloudPCL;
89 
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
90 
 
44 jakw 91 
    // Determine transform in world (camera0) coordinate system
92 
    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
93 
    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
94 
    cv::Mat R;
95 
    cv::Rodrigues(rot_rvec, R);
96 
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
97 
    smPointCloud.T = calibration.Rr.t()*cv::Matx33f(R)*calibration.Tr - calibration.Rr.t()*calibration.Tr;
98 
 
9 jakw 99 
    // Emit result
42 jakw 100 
    emit newPointCloud(smPointCloud);
9 jakw 101 
 
27 jakw 102 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
9 jakw 103 
 
104 
}
105 
 
27 jakw 106 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
24 jakw 107 
 
27 jakw 108 
    // Process sequentially
109 
    for(int i=0; i<frameSequences.size(); i++){
110 
        reconstructPointCloud(frameSequences[i]);
24 jakw 111 
    }
112 
 
113 
}
114 
 
41 jakw 115 
void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){
24 jakw 116 
 
41 jakw 117 
    cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));
118 
    cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));
119 
 
120 
    cv::Mat temp(3,4,CV_32F);
121 
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
122 
    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
123 
    cv::Mat P1 = cv::Mat(calibration.K1) * temp;
124 
 
42 jakw 125 
    cv::Mat QMatHomogenous, QMat;
126 
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
127 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
128 
    cvtools::matToPoints3f(QMat, Q);
41 jakw 129 
 
42 jakw 130 
 
41 jakw 131 
}