Subversion Repositories seema-scanner

Rev

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

Rev 33 Rev 36
Line 23... Line 23...
23 
    // Create decoder
 23 
    // Create decoder
24 
    dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();
 24 
    dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();
25 
    if(dir == CodecDirNone)
 25 
    if(dir == CodecDirNone)
26 
        std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;
 26 
        std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;
27 
 
 27 
 
- 
 
 28 
    int resX = settings.value("projector/resX").toInt();
- 
 
 29 
    int resY = settings.value("projector/resY").toInt();
28 
    QString codec = settings.value("codec", "GrayCode").toString();
 30 
    QString codec = settings.value("codec", "GrayCode").toString();
29 
    if(codec == "PhaseShift")
 31 
    if(codec == "PhaseShift")
30 
        decoder = new DecoderPhaseShift(dir);
 32 
        decoder = new DecoderPhaseShift(dir, resX, resY);
31 
    else if(codec == "GrayCode")
 33 
    else if(codec == "GrayCode")
32 
        decoder = new DecoderGrayCode(dir);
 34 
        decoder = new DecoderGrayCode(dir, resX, resY);
33 
    else
 35 
    else
34 
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
 36 
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
35 
 
 37 
 
36 
 
 38 
 
37 
    // Precompute lens correction maps
 39 
    // Precompute lens correction maps
Line 48... Line 50...
48 
 
 50 
 
49 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
 51 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
50 
 
 52 
 
51 
    time.start();
 53 
    time.start();
52 
 
 54 
 
53 
    // Decode frames
 55 
    // Get correspondences
54 
    cv::Mat up0, vp0, up1, vp1, shading0, mask0, shading1, mask1;
 56 
    std::vector<cv::Point2f> q0, q1;
55 
    decoder->decodeFrames(frameSequence.frames0, up0, vp0, mask0, shading0);
 57 
    std::vector<cv::Point3f> color;
56 
    decoder->decodeFrames(frameSequence.frames1, up1, vp1, mask1, shading1);
 58 
    decoder->getCorrespondences(frameSequence.frames0, frameSequence.frames0, q0, q1, color);
57 
 
 59 
 
58 
    // Triangulate
 60 
    // Triangulate
59 
    cv::Mat pointCloud;
 61 
    std::vector<cv::Point3f> Q;
60 
    if(dir == CodecDirBoth)
 - 
 
61 
        triangulateFromUpVp(up0, vp0, mask0, up1, vp1, mask1, pointCloud);
 - 
 
62 
    else if(dir == CodecDirHorizontal)
 - 
 
63 
        triangulateFromUp(up0, mask0, up1, mask1, pointCloud);
 - 
 
64 
    else if(dir == CodecDirVertical)
 - 
 
65 
        triangulateFromVp(vp0, mask0, vp1, mask1, pointCloud);
 - 
 
66 
 
 - 
 
67 
    // Simply use shading information from camera 0 (for now)
 - 
 
68 
    cv::Mat shading = shading0;
 62 
    triangulate(q0, q1, Q);
69 
 
 63 
 
70 
    // Convert point cloud to PCL format
 64 
    // Convert point cloud to PCL format
71 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
 65 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
72 
 
 66 
 
73 
    // Interprete as organized point cloud
 67 
    // Interprete as organized point cloud
74 
    pointCloudPCL->width = pointCloud.cols;
 68 
    pointCloudPCL->width = Q.size();
75 
    pointCloudPCL->height = pointCloud.rows;
 69 
    pointCloudPCL->height = 1;
76 
    pointCloudPCL->is_dense = false;
 70 
    pointCloudPCL->is_dense = false;
77 
 
 71 
 
78 
    pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);
 72 
    pointCloudPCL->points.resize(Q.size());
79 
 
 73 
 
80 
    for(int row=0; row<pointCloud.rows; row++){
 74 
    for(int i=0; i<Q.size(); i++){
81 
        int offset = row * pointCloudPCL->width;
 - 
 
82 
        for(int col=0; col<pointCloud.cols; col++){
 - 
 
83 
            const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
 - 
 
84 
            unsigned char shade = shading.at<unsigned short>(row,col) >> 8;
 - 
 
85 
            pcl::PointXYZRGB point;
 75 
        pcl::PointXYZRGB point;
86 
            point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
 76 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
87 
            point.r = shade; point.g = shade; point.b = shade;
 77 
        point.r = color[i].x; point.g = color[i].y; point.b = color[i].z;
88 
            pointCloudPCL->points[offset + col] = point;
 78 
        pointCloudPCL->points[i] = point;
89 
        }
 - 
 
90 
    }
 79 
    }
91 
 
 80 
 
92 
/*    // stack xyz data
 - 
 
93 
    std::vector<cv::Mat> xyz;
 - 
 
94 
    cv::split(pointCloud, xyz);
 - 
 
95 
    std::vector<cv::Mat> pointCloudChannels;
 - 
 
96 
    pointCloudChannels.push_back(xyz[0]);
 - 
 
97 
    pointCloudChannels.push_back(xyz[1]);
 - 
 
98 
    pointCloudChannels.push_back(xyz[2]);
 - 
 
99 
 
 - 
 
100 
    // 4 byte padding
 - 
 
101 
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
 - 
 
102 
 
 - 
 
103 
    // triple uchar color information
 - 
 
104 
    std::vector<cv::Mat> rgb;
 - 
 
105 
    rgb.push_back(shading);
 - 
 
106 
    rgb.push_back(shading);
 - 
 
107 
    rgb.push_back(shading);
 - 
 
108 
    rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));
 - 
 
109 
 
 - 
 
110 
    cv::Mat rgb8UC4;
 - 
 
111 
    cv::merge(rgb, rgb8UC4);
 - 
 
112 
 
 - 
 
113 
    cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);
 - 
 
114 
 
 - 
 
115 
    pointCloudChannels.push_back(rgb32F);
 - 
 
116 
 
 - 
 
117 
    // 12 bytes padding
 - 
 
118 
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
 - 
 
119 
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
 - 
 
120 
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
 - 
 
121 
 
 - 
 
122 
    // merge channels
 - 
 
123 
    cv::Mat pointCloudPadded;
 - 
 
124 
    cv::merge(pointCloudChannels, pointCloudPadded);
 - 
 
125 
 
 - 
 
126 
    // memcpy everything
 - 
 
127 
    memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));*/
 - 
 
128 
 
 - 
 
129 
 
 - 
 
130 
    // Emit result
 81 
    // Emit result
131 
    emit newPointCloud(pointCloudPCL);
 82 
    emit newPointCloud(pointCloudPCL);
132 
 
 83 
 
133 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 84 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
134 
 
 85 
 
Line 141... Line 92...
141 
        reconstructPointCloud(frameSequences[i]);
 92 
        reconstructPointCloud(frameSequences[i]);
142 
    }
 93 
    }
143 
 
 94 
 
144 
}
 95 
}
145 
 
 96 
 
146 
void SMReconstructionWorker::triangulateFromUp(cv::Mat up0, cv::Mat mask0,cv::Mat up1, cv::Mat mask1,cv::Mat &xyz){}
 97 
void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){}
147 
void SMReconstructionWorker::triangulateFromVp(cv::Mat vp0, cv::Mat mask0, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}
 - 
 
148 
void SMReconstructionWorker::triangulateFromUpVp(cv::Mat up0, cv::Mat vp0, cv::Mat mask0, cv::Mat up1, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}
 - 
 
149 
 
 98 
 
150 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 99 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
151 
 
 100 
 
152 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
 101 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
153 
//    int N = up.rows * up.cols;
 102 
//    int N = up.rows * up.cols;