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


#include "SMReconstructionWorker.h"
 
#include "AlgorithmGrayCode.h"
#include "AlgorithmGrayCodeHorzVert.h"
#include "AlgorithmPhaseShiftTwoFreq.h"
#include "AlgorithmPhaseShiftThreeFreq.h"
#include "AlgorithmLineShift.h"
 
#include <QCoreApplication>
#include <QSettings>
 
#include <iostream>
#include <opencv2/opencv.hpp>
 
#include "cvtools.h"
 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
 
 
void SMReconstructionWorker::setup(){
 
 
}
 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
 
    QSettings settings;
 
    // Get current calibration
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 
    // Create Algorithm
    QString codec = frameSequence.codec;
    int resX = settings.value("projector/resX").toInt();
    int resY = settings.value("projector/resY").toInt();
 
 
    if(codec == "GrayCode")
        algorithm = new AlgorithmGrayCode(resX, resY);
    else if(codec == "GrayCodeHorzVert")
        algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);
    else if(codec == "PhaseShiftTwoFreq")
        algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);
    else if(codec == "PhaseShiftThreeFreq")
        algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);
    else if(codec == "LineShift")
        algorithm = new AlgorithmLineShift(resX, resY);
    else
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
    time.start();
 
    // Get 3D Points
    std::vector<cv::Point3f> Q;
    std::vector<cv::Vec3b> color;
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
 
    // Convert point cloud to PCL format
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 
    pointCloudPCL->width = Q.size();
    pointCloudPCL->height = 1;
    pointCloudPCL->is_dense = true;
 
    pointCloudPCL->points.resize(Q.size());
 
    for(unsigned int i=0; i<Q.size(); i++){
        pcl::PointXYZRGBNormal point;
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
        pointCloudPCL->points[i] = point;
    }
 
    // Estimate surface normals
    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
    //ne.setKSearch(10);
    ne.setRadiusSearch(0.5);
    ne.setViewPoint(0.0, 0.0, 0.0);
    ne.setInputCloud(pointCloudPCLCopy);
    ne.compute(*pointCloudPCL);
 
    // Assemble SMPointCloud data structure
    SMPointCloud smPointCloud;
    smPointCloud.id = frameSequence.id;
    smPointCloud.pointCloud = pointCloudPCL;
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
 
    // Determine transform in world (camera0) coordinate system
    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
    cv::Mat R;
    cv::Rodrigues(rot_rvec, R);
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
    smPointCloud.T = calibration.Rr.t()*cv::Matx33f(R)*calibration.Tr - calibration.Rr.t()*calibration.Tr;
 
    // Emit result
    emit newPointCloud(smPointCloud);
 
    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::Mat QMatHomogenous, QMat;
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
    cvtools::matToPoints3f(QMat, Q);
 
 
}
 

Subversion Repositories seema-scanner

(root)/src/SMReconstructionWorker.cpp – Rev 128 → 137

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