Subversion Repositories seema-scanner

Rev

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

Rev 24 Rev 25
Line 1... Line 1...
1 
#include "SMSMTriangulator.h"
 1 
#include "SMTriangulator.h"
2 
 
 2 
 
3 
#include <QCoreApplication>
 3 
#include <QCoreApplication>
4 
#include <QSettings>
 4 
#include <QSettings>
5 
 
 5 
 
6 
#include <iostream>
 6 
#include <iostream>
7 
#include <opencv2/opencv.hpp>
 7 
#include <opencv2/opencv.hpp>
8 
 
 8 
 
9 
#include <pcl/filters/statistical_outlier_removal.h>
 9 
#include <pcl/filters/statistical_outlier_removal.h>
10 
#include <pcl/io/pcd_io.h>
 10 
#include <pcl/io/pcd_io.h>
11 
 
 11 
 
12 
void SMSMTriangulator::setup(){
 - 
 
13 
 
 12 
 
14 
    // Initialize SMTriangulator with calibration
 13 
SMTriangulator::SMTriangulator(){
15 
    calibration = new SMCalibrationParams;
 - 
 
16 
    calibration->load("calibration.yml");
 - 
 
17 
    SMTriangulator = new SMTriangulator(*calibration, frameWidth, frameHeight);
 - 
 
18 
 
 14 
 
19 
    QSettings settings("SLStudio");
 15 
    // Precompute lens correction maps
20 
    writeToDisk = settings.value("writeToDisk/pointclouds",false).toBool();
 16 
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
- 
 
 17 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1, lensMap2);
21 
 
 18 
 
- 
 
 19 
    //cv::Mat map1, map2;
- 
 
 20 
    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
- 
 
 21 
    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
- 
 
 22 
    //cv::imwrite("map1.png", map1);
- 
 
 23 
    //cv::imwrite("map2.png", map2);
22 
}
 24 
}
23 
 
 25 
 
24 
 
 - 
 
25 
void SMSMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
 26 
void SMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
26 
 
 27 
 
27 
    time.start();
 28 
    time.start();
28 
 
 29 
 
29 
    // Reconstruct point cloud
 30 
    // Reconstruct point cloud
30 
    cv::Mat pointCloud;
 31 
    cv::Mat pointCloud;
31 
    cv::Mat empty;
 32 
    cv::Mat empty;
32 
    SMTriangulator->triangulate(up, vp, mask, shading, pointCloud);
 33 
    triangulate(up, vp, mask, shading, pointCloud);
33 
 
 34 
 
34 
    std::vector<cv::Mat> xyz;
 35 
    std::vector<cv::Mat> xyz;
35 
    cv::split(pointCloud, xyz);
 36 
    cv::split(pointCloud, xyz);
36 
//    emit imshow("x", xyz[0], 1400, 100);
 37 
//    emit imshow("x", xyz[0], 1400, 100);
37 
//    emit imshow("y", xyz[1], 1400, 450);
 38 
//    emit imshow("y", xyz[1], 1400, 450);
Line 104... Line 105...
104 
    // Emit result
 105 
    // Emit result
105 
    emit newPointCloud(pointCloudPCL);
 106 
    emit newPointCloud(pointCloudPCL);
106 
 
 107 
 
107 
    std::cout << "SMTriangulator: " << time.elapsed() << "ms" << std::endl;
 108 
    std::cout << "SMTriangulator: " << time.elapsed() << "ms" << std::endl;
108 
 
 109 
 
109 
    if(writeToDisk){
 - 
 
110 
        QString fileName = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmsszzz");
 - 
 
111 
        fileName.append(".pcd");
 - 
 
112 
        pcl::io::savePCDFileBinary(fileName.toStdString(), *pointCloudPCL);
 - 
 
113 
    }
 - 
 
114 
 
 - 
 
115 
    //emit finished();
 110 
    //emit finished();
116 
}
 111 
}
117 
 
 112 
 
118 
 
 - 
 
119 
SMTriangulator::SMTriangulator(SMCalibrationParams _calibration, unsigned int frameWidth, unsigned int frameHeight) : calibration(_calibration){
 - 
 
120 
 
 - 
 
121 
    // Precompute uc, vc maps
 - 
 
122 
    uc.create(frameHeight, frameWidth, CV_32F);
 - 
 
123 
    vc.create(frameHeight, frameWidth, CV_32F);
 - 
 
124 
 
 - 
 
125 
    for(unsigned int row=0; row<frameHeight; row++){
 - 
 
126 
        for(unsigned int col=0; col<frameWidth; col++){
 - 
 
127 
            uc.at<float>(row, col) = col;
 - 
 
128 
            vc.at<float>(row, col) = row;
 - 
 
129 
        }
 - 
 
130 
    }
 - 
 
131 
 
 - 
 
132 
    // Precompute determinant tensor
 - 
 
133 
    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
 - 
 
134 
    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
 - 
 
135 
 
 - 
 
136 
    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
 - 
 
137 
    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
 - 
 
138 
    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
 - 
 
139 
    Pp = cv::Mat(calibration.Kp) * temp;
 - 
 
140 
 
 - 
 
141 
    cv::Mat e = cv::Mat::eye(4, 4, CV_32F);
 - 
 
142 
 
 - 
 
143 
    int sz[] = {4, 3, 3, 3};
 - 
 
144 
    cv::Mat C(4, sz, CV_32F, cv::Scalar::all(0));
 - 
 
145 
    for(int k=0; k<4; k++){
 - 
 
146 
        for(int i=0; i<3; i++){
 - 
 
147 
            for(int j=0; j<3; j++){
 - 
 
148 
                for(int l=0; l<3; l++){
 - 
 
149 
                    cv::Mat op(4, 4, CV_32F);
 - 
 
150 
                    Pc.row(i).copyTo(op.row(0));
 - 
 
151 
                    Pc.row(j).copyTo(op.row(1));
 - 
 
152 
                    Pp.row(l).copyTo(op.row(2));
 - 
 
153 
                    e.row(k).copyTo(op.row(3));
 - 
 
154 
                    C.at<float>(cv::Vec4i(k,i,j,l)) = cv::determinant(op.t());
 - 
 
155 
                }
 - 
 
156 
            }
 - 
 
157 
        }
 - 
 
158 
    }
 - 
 
159 
    determinantTensor = C;
 - 
 
160 
 
 - 
 
161 
    // Precompute lens correction maps
 - 
 
162 
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
 - 
 
163 
    cv::initUndistortRectifyMap(calibration.Kc, calibration.kc, eye, calibration.Kc, cv::Size(frameWidth, frameHeight), CV_32FC1, lensMap1, lensMap2);
 - 
 
164 
 
 - 
 
165 
    //cv::Mat map1, map2;
 - 
 
166 
    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
 - 
 
167 
    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
 - 
 
168 
    //cv::imwrite("map1.png", map1);
 - 
 
169 
    //cv::imwrite("map2.png", map2);
 - 
 
170 
}
 - 
 
171 
 
 - 
 
172 
void SMTriangulator::triangulate(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading, cv::Mat &pointCloud){
 113 
void SMTriangulator::triangulate(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading, cv::Mat &pointCloud){
173 
 
 114 
 
174 
    // Undistort up, mask and shading
 115 
    // Undistort up, mask and shading
175 
    if(!up.empty()){
 116 
    if(!up.empty()){
176 
        cv::Mat upUndistort;
 117 
        cv::Mat upUndistort;
Line 244... Line 185...
244 
 
 185 
 
245 
}
 186 
}
246 
 
 187 
 
247 
void SMTriangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 188 
void SMTriangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
248 
 
 189 
 
249 
    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
 190 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
250 
    int N = up.rows * up.cols;
 191 
//    int N = up.rows * up.cols;
251 
 
 192 
 
252 
    cv::Mat projPointsCam(2, N, CV_32F);
 193 
//    cv::Mat projPointsCam(2, N, CV_32F);
253 
    uc.reshape(0,1).copyTo(projPointsCam.row(0));
 194 
//    uc.reshape(0,1).copyTo(projPointsCam.row(0));
254 
    vc.reshape(0,1).copyTo(projPointsCam.row(1));
 195 
//    vc.reshape(0,1).copyTo(projPointsCam.row(1));
255 
 
 196 
 
256 
    cv::Mat projPointsProj(2, N, CV_32F);
 197 
//    cv::Mat projPointsProj(2, N, CV_32F);
257 
    up.reshape(0,1).copyTo(projPointsProj.row(0));
 198 
//    up.reshape(0,1).copyTo(projPointsProj.row(0));
258 
    vp.reshape(0,1).copyTo(projPointsProj.row(1));
 199 
//    vp.reshape(0,1).copyTo(projPointsProj.row(1));
259 
 
 200 
 
260 
    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
 201 
//    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
261 
    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
 202 
//    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
262 
 
 203 
 
263 
    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
 204 
//    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
264 
    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
 205 
//    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
265 
    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
 206 
//    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
266 
    Pp = cv::Mat(calibration.Kp) * temp;
 207 
//    Pp = cv::Mat(calibration.Kp) * temp;
267 
 
 208 
 
268 
    cv::Mat xyzw;
 209 
//    cv::Mat xyzw;
269 
    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
 210 
//    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
270 
 
 211 
 
271 
    xyz.create(3, N, CV_32F);
 212 
//    xyz.create(3, N, CV_32F);
272 
    for(int i=0; i<N; i++){
 213 
//    for(int i=0; i<N; i++){
273 
        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
 214 
//        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
274 
        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
 215 
//        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
275 
        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
 216 
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
276 
    }
 217 
//    }
277 
 
 218 
 
278 
    xyz = xyz.t();
 219 
//    xyz = xyz.t();
279 
    xyz = xyz.reshape(3, up.rows);
 220 
//    xyz = xyz.reshape(3, up.rows);
280 
}
 221 
}
281 
 
 222 
 
282 
SMSMTriangulator::~SMSMTriangulator(){
 223 
SMTriangulator::~SMTriangulator(){
283 
    delete calibration;
 - 
 
284 
    delete SMTriangulator;
 - 
 
285 
 
 224 
 
286 
    std::cout<<"SMTriangulatorWorker deleted\n"<<std::flush;
 225 
    std::cout << "SMTriangulator deleted.\n" << std::flush;
287 
}
 226 
}
288 
 
 227