WebSVN – seema-scanner – Diff – /src/SMReconstructionWorker.cpp


#include "SMReconstructionWorker.h"
 
#include "AlgorithmGrayCode.h"
#include "AlgorithmPhaseShift.h"
 
#include <QCoreApplication>
#include <QSettings>
 
#include <iostream>
#include <opencv2/opencv.hpp>
 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
 
 
void SMReconstructionWorker::setup(){
 
    QSettings settings;
 
    // Get current calibration
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 
    // Create Algorithm
    dir = (CodingDir)settings.value("pattern/direction", CodingDirHorizontal).toInt();
    if(dir == CodingDirNone)
        std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;
 
    int resX = settings.value("projector/resX").toInt();
    int resY = settings.value("projector/resY").toInt();
    QString codec = settings.value("codec", "GrayCode").toString();
    if(codec == "PhaseShift")
        algorithm = new AlgorithmPhaseShift(resX, resY, dir);
    else if(codec == "GrayCode")
        algorithm = new AlgorithmGrayCode(resX, resY, dir);
    else
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
 
 
    // Precompute lens correction maps
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);
 
    //cv::Mat mapHorz, mapVert;
    //cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);
    //cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);
    //cv::imwrite("mapHorz.png", mapHorz);
    //cv::imwrite("mapVert.png", mapVert);
}
 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
 
    time.start();
 
    // Get correspondences
    std::vector<cv::Point2f> q0, q1;
    std::vector<cv::Point3f> color;
    algorithm->getCorrespondences(calibration, frameSequence.frames0, frameSequence.frames0, q0, q1, color);
 
    // Triangulate
    std::vector<cv::Point3f> Q;
    triangulate(q0, q1, Q);
 
    // Convert point cloud to PCL format
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
 
    // Interprete as organized point cloud
    pointCloudPCL->width = Q.size();
    pointCloudPCL->height = 1;
    pointCloudPCL->is_dense = false;
 
    pointCloudPCL->points.resize(Q.size());
 
    for(int i=0; i<Q.size(); i++){
        pcl::PointXYZRGB point;
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
        point.r = color[i].x; point.g = color[i].y; point.b = color[i].z;
        pointCloudPCL->points[i] = point;
    }
 
    // Emit result
    emit newPointCloud(pointCloudPCL);
 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 
}
 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
 
    // Process sequentially
    for(int i=0; i<frameSequences.size(); i++){
        reconstructPointCloud(frameSequences[i]);
    }
 
}
 
void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){
 
    cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));
    cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));
 
    cv::Mat temp(3,4,CV_32F);
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
    cv::Mat P1 = cv::Mat(calibration.K1) * temp;
 
    cv::triangulatePoints(P0, P1, q0, q1, Q);
 
}
 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
//    int N = up.rows * up.cols;
 
//    cv::Mat projPointsCam(2, N, CV_32F);
//    uc.reshape(0,1).copyTo(projPointsCam.row(0));
//    vc.reshape(0,1).copyTo(projPointsCam.row(1));
 
//    cv::Mat projPointsProj(2, N, CV_32F);
//    up.reshape(0,1).copyTo(projPointsProj.row(0));
//    vp.reshape(0,1).copyTo(projPointsProj.row(1));
 
//    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
//    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
 
//    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
//    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
//    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
//    Pp = cv::Mat(calibration.Kp) * temp;
 
//    cv::Mat xyzw;
//    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
 
//    xyz.create(3, N, CV_32F);
//    for(int i=0; i<N; i++){
//        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
//        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
//    }
 
//    xyz = xyz.t();
//    xyz = xyz.reshape(3, up.rows);
//}
 
 

Subversion Repositories seema-scanner

(root)/src/SMReconstructionWorker.cpp – Rev 36 → 41

Rev 36		Rev 41
1	`#include "SMReconstructionWorker.h"`	1	`#include "SMReconstructionWorker.h"`
2		2
3	`#include "CodecGrayCode.h"`	3	`#include "AlgorithmGrayCode.h"`
4	`#include "CodecPhaseShift.h"`	4	`#include "AlgorithmPhaseShift.h"`
5		5
6	`#include <QCoreApplication>`	6	`#include <QCoreApplication>`
7	`#include <QSettings>`	7	`#include <QSettings>`
8		8
9	`#include <iostream>`	9	`#include <iostream>`
10	`#include <opencv2/opencv.hpp>`	10	`#include <opencv2/opencv.hpp>`
11		11
12	`#include <pcl/filters/statistical_outlier_removal.h>`	12	`#include <pcl/filters/statistical_outlier_removal.h>`
13	`#include <pcl/io/pcd_io.h>`	13	`#include <pcl/io/pcd_io.h>`
14		14
15		15
16	`void SMReconstructionWorker::setup(){`	16	`void SMReconstructionWorker::setup(){`
17		17
18	`QSettings settings;`	18	`QSettings settings;`
19		19
20	`// Get current calibration`	20	`// Get current calibration`
21	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`	21	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`
22		22
23	`// Create decoder`	23	`// Create Algorithm`
24	`dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();`	24	`dir = (CodingDir)settings.value("pattern/direction", CodingDirHorizontal).toInt();`
25	`if(dir == CodecDirNone)`	25	`if(dir == CodingDirNone)`
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();`	28	`int resX = settings.value("projector/resX").toInt();`
29	`int resY = settings.value("projector/resY").toInt();`	29	`int resY = settings.value("projector/resY").toInt();`
30	`QString codec = settings.value("codec", "GrayCode").toString();`	30	`QString codec = settings.value("codec", "GrayCode").toString();`
31	`if(codec == "PhaseShift")`	31	`if(codec == "PhaseShift")`
32	`decoder = new DecoderPhaseShift(dir, resX, resY);`	32	`algorithm = new AlgorithmPhaseShift(resX, resY, dir);`
33	`else if(codec == "GrayCode")`	33	`else if(codec == "GrayCode")`
34	`decoder = new DecoderGrayCode(dir, resX, resY);`	34	`algorithm = new AlgorithmGrayCode(resX, resY, dir);`
35	`else`	35	`else`
36	`std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;`	36	`std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;`
37		37
38		38
39	`// Precompute lens correction maps`	39	`// Precompute lens correction maps`
40	`cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);`	40	`cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);`
41	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);`	41	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);`
42	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);`	42	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);`
43		43
44	`//cv::Mat mapHorz, mapVert;`	44	`//cv::Mat mapHorz, mapVert;`
45	`//cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);`	45	`//cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);`
46	`//cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);`	46	`//cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);`
47	`//cv::imwrite("mapHorz.png", mapHorz);`	47	`//cv::imwrite("mapHorz.png", mapHorz);`
48	`//cv::imwrite("mapVert.png", mapVert);`	48	`//cv::imwrite("mapVert.png", mapVert);`
49	`}`	49	`}`
50		50
51	`void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){`	51	`void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){`
52		52
53	`time.start();`	53	`time.start();`
54		54
55	`// Get correspondences`	55	`// Get correspondences`
56	`std::vector<cv::Point2f> q0, q1;`	56	`std::vector<cv::Point2f> q0, q1;`
57	`std::vector<cv::Point3f> color;`	57	`std::vector<cv::Point3f> color;`
58	`decoder->getCorrespondences(frameSequence.frames0, frameSequence.frames0, q0, q1, color);`	58	`algorithm->getCorrespondences(calibration, frameSequence.frames0, frameSequence.frames0, q0, q1, color);`
59		59
60	`// Triangulate`	60	`// Triangulate`
61	`std::vector<cv::Point3f> Q;`	61	`std::vector<cv::Point3f> Q;`
62	`triangulate(q0, q1, Q);`	62	`triangulate(q0, q1, Q);`
63		63
64	`// Convert point cloud to PCL format`	64	`// Convert point cloud to PCL format`
65	`pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);`	65	`pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);`
66		66
67	`// Interprete as organized point cloud`	67	`// Interprete as organized point cloud`
68	`pointCloudPCL->width = Q.size();`	68	`pointCloudPCL->width = Q.size();`
69	`pointCloudPCL->height = 1;`	69	`pointCloudPCL->height = 1;`
70	`pointCloudPCL->is_dense = false;`	70	`pointCloudPCL->is_dense = false;`
71		71
72	`pointCloudPCL->points.resize(Q.size());`	72	`pointCloudPCL->points.resize(Q.size());`
73		73
74	`for(int i=0; i<Q.size(); i++){`	74	`for(int i=0; i<Q.size(); i++){`
75	`pcl::PointXYZRGB point;`	75	`pcl::PointXYZRGB point;`
76	`point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;`	76	`point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;`
77	`point.r = color[i].x; point.g = color[i].y; point.b = color[i].z;`	77	`point.r = color[i].x; point.g = color[i].y; point.b = color[i].z;`
78	`pointCloudPCL->points[i] = point;`	78	`pointCloudPCL->points[i] = point;`
79	`}`	79	`}`
80		80
81	`// Emit result`	81	`// Emit result`
82	`emit newPointCloud(pointCloudPCL);`	82	`emit newPointCloud(pointCloudPCL);`
83		83
84	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`	84	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`
85		85
86	`}`	86	`}`
87		87
88	`void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){`	88	`void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){`
89		89
90	`// Process sequentially`	90	`// Process sequentially`
91	`for(int i=0; i<frameSequences.size(); i++){`	91	`for(int i=0; i<frameSequences.size(); i++){`
92	`reconstructPointCloud(frameSequences[i]);`	92	`reconstructPointCloud(frameSequences[i]);`
93	`}`	93	`}`
94		94
95	`}`	95	`}`
96		96
97	`void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){}`	97	`void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){`
-		98
-		99	`cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));`
-		100	`cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));`
-		101
-		102	`cv::Mat temp(3,4,CV_32F);`
-		103	`cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`
-		104	`cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`
-		105	`cv::Mat P1 = cv::Mat(calibration.K1) * temp;`
-		106
-		107	`cv::triangulatePoints(P0, P1, q0, q1, Q);`
-		108
-		109	`}`
98		110
99	`//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){`	111	`//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){`
100		112
101	`// std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;`	113	`// std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;`
102	`// int N = up.rows * up.cols;`	114	`// int N = up.rows * up.cols;`
103		115
104	`// cv::Mat projPointsCam(2, N, CV_32F);`	116	`// cv::Mat projPointsCam(2, N, CV_32F);`
105	`// uc.reshape(0,1).copyTo(projPointsCam.row(0));`	117	`// uc.reshape(0,1).copyTo(projPointsCam.row(0));`
106	`// vc.reshape(0,1).copyTo(projPointsCam.row(1));`	118	`// vc.reshape(0,1).copyTo(projPointsCam.row(1));`
107		119
108	`// cv::Mat projPointsProj(2, N, CV_32F);`	120	`// cv::Mat projPointsProj(2, N, CV_32F);`
109	`// up.reshape(0,1).copyTo(projPointsProj.row(0));`	121	`// up.reshape(0,1).copyTo(projPointsProj.row(0));`
110	`// vp.reshape(0,1).copyTo(projPointsProj.row(1));`	122	`// vp.reshape(0,1).copyTo(projPointsProj.row(1));`
111		123
112	`// cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));`	124	`// cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));`
113	`// cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));`	125	`// cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));`
114		126
115	`// cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);`	127	`// cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);`
116	`// cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`	128	`// cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`
117	`// cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`	129	`// cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`
118	`// Pp = cv::Mat(calibration.Kp) * temp;`	130	`// Pp = cv::Mat(calibration.Kp) * temp;`
119		131
120	`// cv::Mat xyzw;`	132	`// cv::Mat xyzw;`
121	`// cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);`	133	`// cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);`
122		134
123	`// xyz.create(3, N, CV_32F);`	135	`// xyz.create(3, N, CV_32F);`
124	`// for(int i=0; i<N; i++){`	136	`// for(int i=0; i<N; i++){`
125	`// xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);`	137	`// xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);`
126	`// xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);`	138	`// xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);`
127	`// xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);`	139	`// xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);`
128	`// }`	140	`// }`
129		141
130	`// xyz = xyz.t();`	142	`// xyz = xyz.t();`
131	`// xyz = xyz.reshape(3, up.rows);`	143	`// xyz = xyz.reshape(3, up.rows);`
132	`//}`	144	`//}`
133		145
134		146