Subversion Repositories seema-scanner

Rev

Rev 25 | Rev 28 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 25 Rev 27
Line 1... Line 1...
1 
#include "SMTriangulator.h"
 1 
#include "SMReconstructionWorker.h"
- 
 
 2 
 
- 
 
 3 
#include "CodecGrayCode.h"
- 
 
 4 
#include "CodecPhaseShift.h"
2 
 
 5 
 
3 
#include <QCoreApplication>
 6 
#include <QCoreApplication>
4 
#include <QSettings>
 7 
#include <QSettings>
5 
 
 8 
 
6 
#include <iostream>
 9 
#include <iostream>
Line 8... Line 11...
8 
 
 11 
 
9 
#include <pcl/filters/statistical_outlier_removal.h>
 12 
#include <pcl/filters/statistical_outlier_removal.h>
10 
#include <pcl/io/pcd_io.h>
 13 
#include <pcl/io/pcd_io.h>
11 
 
 14 
 
12 
 
 15 
 
13 
SMTriangulator::SMTriangulator(){
 16 
void SMReconstructionWorker::setup(){
- 
 
 17 
 
- 
 
 18 
    QSettings settings;
- 
 
 19 
 
- 
 
 20 
    // Get current calibration
- 
 
 21 
    calibration = settings.value("calibration").value<SMCalibrationParameters>();
- 
 
 22 
 
- 
 
 23 
    // Create decoder
- 
 
 24 
    dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();
- 
 
 25 
    if(dir == CodecDirNone)
- 
 
 26 
        std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;
- 
 
 27 
 
- 
 
 28 
    QString codec = settings.value("codec", "GrayCode").toString();
- 
 
 29 
    if(codec == "PhaseShift")
- 
 
 30 
        decoder = new DecoderPhaseShift(dir);
- 
 
 31 
    else if(codec == "GrayCode")
- 
 
 32 
        decoder = new DecoderGrayCode(dir);
- 
 
 33 
    else
- 
 
 34 
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
- 
 
 35 
 
14 
 
 36 
 
15 
    // Precompute lens correction maps
 37 
    // Precompute lens correction maps
16 
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
 38 
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
- 
 
 39 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);
17 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1, lensMap2);
 40 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);
18 
 
 41 
 
19 
    //cv::Mat map1, map2;
 42 
    //cv::Mat mapHorz, mapVert;
20 
    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
 43 
    //cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);
21 
    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
 44 
    //cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);
22 
    //cv::imwrite("map1.png", map1);
 45 
    //cv::imwrite("mapHorz.png", mapHorz);
23 
    //cv::imwrite("map2.png", map2);
 46 
    //cv::imwrite("mapVert.png", mapVert);
24 
}
 47 
}
25 
 
 48 
 
26 
void SMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
 49 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
27 
 
 50 
 
28 
    time.start();
 51 
    time.start();
29 
 
 52 
 
30 
    // Reconstruct point cloud
 53 
    // Decode frames
- 
 
 54 
    cv::Mat up0, vp0, up1, vp1, shading0, mask0, shading1, mask1;
- 
 
 55 
    decoder->decodeFrames(frameSequence.frames0, up0, vp0, mask0, shading0);
- 
 
 56 
    decoder->decodeFrames(frameSequence.frames1, up1, vp1, mask1, shading1);
- 
 
 57 
 
- 
 
 58 
    // Triangulate
31 
    cv::Mat pointCloud;
 59 
    cv::Mat pointCloud;
32 
    cv::Mat empty;
 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)
33 
    triangulate(up, vp, mask, shading, pointCloud);
 65 
        triangulateFromVp(vp0, mask0, vp1, mask1, pointCloud);
34 
 
 66 
 
35 
    std::vector<cv::Mat> xyz;
 67 
    // Simply use shading information from camera 0 (for now)
36 
    cv::split(pointCloud, xyz);
 68 
    cv::Mat shading = shading0;
37 
//    emit imshow("x", xyz[0], 1400, 100);
 - 
 
38 
//    emit imshow("y", xyz[1], 1400, 450);
 - 
 
39 
//    emit imshow("z", xyz[2], 1400, 800);
 - 
 
40 
 
 69 
 
41 
    // Convert point cloud to PCL format
 70 
    // Convert point cloud to PCL format
42 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
 71 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
43 
 
 72 
 
44 
    // Interprete as organized point cloud
 73 
    // Interprete as organized point cloud
Line 46... Line 75...
46 
    pointCloudPCL->height = pointCloud.rows;
 75 
    pointCloudPCL->height = pointCloud.rows;
47 
    pointCloudPCL->is_dense = false;
 76 
    pointCloudPCL->is_dense = false;
48 
 
 77 
 
49 
    pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);
 78 
    pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);
50 
 
 79 
 
51 
    //    for(int col=0; col<pointCloud.cols; col++){
 80 
    for(int row=0; row<pointCloud.rows; row++){
- 
 
 81 
        int offset = row * pointCloudPCL->width;
52 
    //        for(int row=0; row<pointCloud.rows; row++){
 82 
        for(int col=0; col<pointCloud.cols; col++){
53 
    //            const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
 83 
            const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
54 
    //            unsigned char shade = shading.at<unsigned char>(row,col);
 84 
            unsigned char shade = shading.at<unsigned short>(row,col) >> 8;
55 
    //            pcl::PointXYZRGBRGB point;
 85 
            pcl::PointXYZRGB point;
56 
    //            point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
 86 
            point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
57 
    //            point.r = shade; point.g = shade; point.b = shade;
 87 
            point.r = shade; point.g = shade; point.b = shade;
58 
    //            pointCloudPCL->at(col, row) = point;
 88 
            pointCloudPCL->points[offset + col] = point;
59 
    //        }
 89 
        }
60 
    //    }
 90 
    }
61 
 
 91 
 
62 
    // stack xyz data
 92 
/*    // stack xyz data
- 
 
 93 
    std::vector<cv::Mat> xyz;
- 
 
 94 
    cv::split(pointCloud, xyz);
63 
    std::vector<cv::Mat> pointCloudChannels;
 95 
    std::vector<cv::Mat> pointCloudChannels;
64 
    pointCloudChannels.push_back(xyz[0]);
 96 
    pointCloudChannels.push_back(xyz[0]);
65 
    pointCloudChannels.push_back(xyz[1]);
 97 
    pointCloudChannels.push_back(xyz[1]);
66 
    pointCloudChannels.push_back(xyz[2]);
 98 
    pointCloudChannels.push_back(xyz[2]);
67 
 
 99 
 
Line 90... Line 122...
90 
    // merge channels
 122 
    // merge channels
91 
    cv::Mat pointCloudPadded;
 123 
    cv::Mat pointCloudPadded;
92 
    cv::merge(pointCloudChannels, pointCloudPadded);
 124 
    cv::merge(pointCloudChannels, pointCloudPadded);
93 
 
 125 
 
94 
    // memcpy everything
 126 
    // memcpy everything
95 
    memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));
 127 
    memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));*/
96 
 
 128 
 
97 
//    // filtering
 - 
 
98 
//    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> filter;
 - 
 
99 
//    filter.setMeanK(5);
 - 
 
100 
//    filter.setStddevMulThresh(1.0);
 - 
 
101 
//    filter.setInputCloud(pointCloudPCL);
 - 
 
102 
//    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
 - 
 
103 
//    filter.filter(*pointCloudFiltered);
 - 
 
104 
 
 129 
 
105 
    // Emit result
 130 
    // Emit result
106 
    emit newPointCloud(pointCloudPCL);
 131 
    emit newPointCloud(pointCloudPCL);
107 
 
 132 
 
108 
    std::cout << "SMTriangulator: " << time.elapsed() << "ms" << std::endl;
 133 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
109 
 
 - 
 
110 
    //emit finished();
 - 
 
111 
}
 - 
 
112 
 
 - 
 
113 
void SMTriangulator::triangulate(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading, cv::Mat &pointCloud){
 - 
 
114 
 
 - 
 
115 
    // Undistort up, mask and shading
 - 
 
116 
    if(!up.empty()){
 - 
 
117 
        cv::Mat upUndistort;
 - 
 
118 
        cv::remap(up, upUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
 - 
 
119 
        up = upUndistort;
 - 
 
120 
    }
 - 
 
121 
    if(!vp.empty()){
 - 
 
122 
        cv::Mat vpUndistort;
 - 
 
123 
        cv::remap(vp, vpUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
 - 
 
124 
        vp = vpUndistort;
 - 
 
125 
    }
 - 
 
126 
 
 - 
 
127 
    cv::Mat maskUndistort, shadingUndistort;
 - 
 
128 
    cv::remap(mask, maskUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
 - 
 
129 
    cv::remap(shading, shadingUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
 - 
 
130 
    mask = maskUndistort;
 - 
 
131 
    shading = shadingUndistort;
 - 
 
132 
 
 - 
 
133 
    // Triangulate
 - 
 
134 
    cv::Mat xyz;
 - 
 
135 
    if(!up.empty() && vp.empty())
 - 
 
136 
        triangulateFromUp(up, xyz);
 - 
 
137 
    else if(!vp.empty() && up.empty())
 - 
 
138 
        triangulateFromVp(vp, xyz);
 - 
 
139 
    else if(!up.empty() && !vp.empty())
 - 
 
140 
        triangulateFromUpVp(up, vp, xyz);
 - 
 
141 
 
 - 
 
142 
    // Merge and mask
 - 
 
143 
    pointCloud = cv::Mat(up.size(), CV_32FC3, cv::Scalar(NAN, NAN, NAN));
 - 
 
144 
    xyz.copyTo(pointCloud, mask);
 - 
 
145 
 
 134 
 
146 
}
 135 
}
147 
 
 136 
 
148 
void SMTriangulator::triangulateFromUp(cv::Mat &up, cv::Mat &xyz){
 137 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
149 
 
 138 
 
150 
    // Solve for xyzw using determinant tensor
 - 
 
151 
    cv::Mat C = determinantTensor;
 139 
    // Process sequentially
152 
    std::vector<cv::Mat> xyzw(4);
 - 
 
153 
    for(unsigned int i=0; i<4; i++){
 140 
    for(int i=0; i<frameSequences.size(); i++){
154 
        xyzw[i].create(up.size(), CV_32F);
 141 
        reconstructPointCloud(frameSequences[i]);
155 
        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 -
 - 
 
156 
                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);
 - 
 
157 
    }
 142 
    }
158 
 
 143 
 
159 
    // Convert to non homogenous coordinates
 - 
 
160 
    for(unsigned int i=0; i<3; i++)
 - 
 
161 
        xyzw[i] /= xyzw[3];
 - 
 
162 
 
 - 
 
163 
    // Merge and mask
 - 
 
164 
    cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
 - 
 
165 
 
 - 
 
166 
}
 144 
}
167 
 
 145 
 
168 
void SMTriangulator::triangulateFromVp(cv::Mat &vp, cv::Mat &xyz){
 146 
void triangulateFromUp(cv::Mat up0, cv::Mat mask0,cv::Mat up1, cv::Mat mask1,cv::Mat &xyz);
169 
 
 - 
 
170 
    // Solve for xyzw using determinant tensor
 - 
 
171 
    cv::Mat C = determinantTensor;
 - 
 
172 
    std::vector<cv::Mat> xyzw(4);
 - 
 
173 
    for(unsigned int i=0; i<4; i++){
 - 
 
174 
        xyzw[i].create(vp.size(), CV_32F);
 - 
 
175 
        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 -
 147 
void triangulateFromVp(cv::Mat vp0, cv::Mat mask0, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz);
176 
                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);
 148 
void triangulateFromUpVp(cv::Mat up0, cv::Mat vp0, cv::Mat mask0, cv::Mat up1, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz);
177 
    }
 - 
 
178 
 
 149 
 
179 
    // Convert to non homogenous coordinates
 - 
 
180 
    for(unsigned int i=0; i<3; i++)
 - 
 
181 
        xyzw[i] /= xyzw[3];
 - 
 
182 
 
 - 
 
183 
    // Merge and mask
 - 
 
184 
    cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
 - 
 
185 
 
 - 
 
186 
}
 - 
 
187 
 
 - 
 
188 
void SMTriangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 150 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
189 
 
 151 
 
190 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
 152 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
191 
//    int N = up.rows * up.cols;
 153 
//    int N = up.rows * up.cols;
192 
 
 154 
 
193 
//    cv::Mat projPointsCam(2, N, CV_32F);
 155 
//    cv::Mat projPointsCam(2, N, CV_32F);
Line 216... Line 178...
216 
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
 178 
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
217 
//    }
 179 
//    }
218 
 
 180 
 
219 
//    xyz = xyz.t();
 181 
//    xyz = xyz.t();
220 
//    xyz = xyz.reshape(3, up.rows);
 182 
//    xyz = xyz.reshape(3, up.rows);
221 
}
 183 
//}
222 
 
 - 
 
223 
SMTriangulator::~SMTriangulator(){
 - 
 
224 
 
 - 
 
225 
    std::cout << "SMTriangulator deleted.\n" << std::flush;
 - 
 
226 
}
 - 
 
227 
 
 184