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


#include "SMReconstructionWorker.h"
 
#include "AlgorithmGrayCode.h"
#include "AlgorithmGrayCodeHorzVert.h"
#include "AlgorithmPhaseShiftTwoFreq.h"
#include "AlgorithmPhaseShiftThreeFreq.h"
#include "AlgorithmPhaseShiftEmbedded.h"
#include "AlgorithmLineShift.h"
 
#include <QCoreApplication>
#include <QSettings>
 
#include <iostream>
#include <opencv2/opencv.hpp>
 
#include "cvtools.h"
#include <opencv2/core/eigen.hpp>
 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/common/transforms.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 == "PhaseShiftEmbedded")
        algorithm = new AlgorithmPhaseShiftEmbedded(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;
    }
 
//    // Transform point cloud to rotation axis coordinate system
//    cv::Mat TRCV(3, 4, CV_32F);
//    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
//    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
//    Eigen::Affine3f TR;
//    cv::cv2eigen(TRCV, TR.matrix());
//    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
 
//    // Estimate surface normals (does not produce proper normals...)
//    std::cout << "Estimating normals..." << std::endl;
//    pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
//    pcl::copyPointCloud(*pointCloudPCL, *points);
//    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
//    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
//    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
//    tree->setInputCloud(points);
//    ne.setSearchMethod(tree);
//    ne.setRadiusSearch(1.0);
//    //ne.setKSearch(50);
//    ne.setViewPoint(0.0, 0.0, 0.0);
//    ne.setInputCloud(points);
//    ne.compute(*normals);
//    pcl::copyPointCloud(*normals, *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;
 
 
//    // 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 = cv::Matx33f(R);
//    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);
 
    // Emit result
    emit newPointCloud(smPointCloud);
 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
}
 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
 
    // Process sequentially
    for(unsigned 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 185 → 192

Rev 185		Rev 192
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 "AlgorithmPhaseShiftEmbedded.h"`
7	`#include "AlgorithmLineShift.h"`	8	`#include "AlgorithmLineShift.h"`
8		9
9	`#include <QCoreApplication>`	10	`#include <QCoreApplication>`
10	`#include <QSettings>`	11	`#include <QSettings>`
11		12
12	`#include <iostream>`	13	`#include <iostream>`
13	`#include <opencv2/opencv.hpp>`	14	`#include <opencv2/opencv.hpp>`
14		15
15	`#include "cvtools.h"`	16	`#include "cvtools.h"`
16	`#include <opencv2/core/eigen.hpp>`	17	`#include <opencv2/core/eigen.hpp>`
17		18
18	`#include <pcl/filters/statistical_outlier_removal.h>`	19	`#include <pcl/filters/statistical_outlier_removal.h>`
19	`#include <pcl/io/pcd_io.h>`	20	`#include <pcl/io/pcd_io.h>`
20	`#include <pcl/features/normal_3d.h>`	21	`#include <pcl/features/normal_3d.h>`
21	`#include <pcl/features/normal_3d_omp.h>`	22	`#include <pcl/features/normal_3d_omp.h>`
22	`#include <pcl/common/transforms.h>`	23	`#include <pcl/common/transforms.h>`
23		24
24		25
25	`void SMReconstructionWorker::setup(){`	26	`void SMReconstructionWorker::setup(){`
26		27
27		28
28	`}`	29	`}`
29		30
30	`void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){`	31	`void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){`
31		32
32	`QSettings settings;`	33	`QSettings settings;`
33		34
34	`// Get current calibration`	35	`// Get current calibration`
35	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`	36	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`
36		37
37	`// Create Algorithm`	38	`// Create Algorithm`
38	`QString codec = frameSequence.codec;`	39	`QString codec = frameSequence.codec;`
39	`int resX = settings.value("projector/resX").toInt();`	40	`int resX = settings.value("projector/resX").toInt();`
40	`int resY = settings.value("projector/resY").toInt();`	41	`int resY = settings.value("projector/resY").toInt();`
41		42
42	`if(codec == "GrayCode")`	43	`if(codec == "GrayCode")`
43	`algorithm = new AlgorithmGrayCode(resX, resY);`	44	`algorithm = new AlgorithmGrayCode(resX, resY);`
44	`else if(codec == "GrayCodeHorzVert")`	45	`else if(codec == "GrayCodeHorzVert")`
45	`algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);`	46	`algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);`
46	`else if(codec == "PhaseShiftTwoFreq")`	47	`else if(codec == "PhaseShiftTwoFreq")`
47	`algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);`	48	`algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);`
48	`else if(codec == "PhaseShiftThreeFreq")`	49	`else if(codec == "PhaseShiftThreeFreq")`
49	`algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);`	50	`algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);`
-		51	`else if(codec == "PhaseShiftEmbedded")`
-		52	`algorithm = new AlgorithmPhaseShiftEmbedded(resX, resY);`
50	`else if(codec == "LineShift")`	53	`else if(codec == "LineShift")`
51	`algorithm = new AlgorithmLineShift(resX, resY);`	54	`algorithm = new AlgorithmLineShift(resX, resY);`
52	`else`	55	`else`
53	`std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;`	56	`std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;`
54		57
55	`time.start();`	58	`time.start();`
56		59
57	`// Get 3D Points`	60	`// Get 3D Points`
58	`std::vector<cv::Point3f> Q;`	61	`std::vector<cv::Point3f> Q;`
59	`std::vector<cv::Vec3b> color;`	62	`std::vector<cv::Vec3b> color;`
60	`algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);`	63	`algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);`
61		64
62	`// Convert point cloud to PCL format`	65	`// Convert point cloud to PCL format`
63	`pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);`	66	`pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);`
64		67
65	`pointCloudPCL->width = Q.size();`	68	`pointCloudPCL->width = Q.size();`
66	`pointCloudPCL->height = 1;`	69	`pointCloudPCL->height = 1;`
67	`pointCloudPCL->is_dense = true;`	70	`pointCloudPCL->is_dense = true;`
68		71
69	`pointCloudPCL->points.resize(Q.size());`	72	`pointCloudPCL->points.resize(Q.size());`
70		73
71	`for(unsigned int i=0; i<Q.size(); i++){`	74	`for(unsigned int i=0; i<Q.size(); i++){`
72	`pcl::PointXYZRGBNormal point;`	75	`pcl::PointXYZRGBNormal point;`
73	`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;`
74	`point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];`	77	`point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];`
75	`pointCloudPCL->points[i] = point;`	78	`pointCloudPCL->points[i] = point;`
76	`}`	79	`}`
77		80
78	`// // Transform point cloud to rotation axis coordinate system`	81	`// // Transform point cloud to rotation axis coordinate system`
79	`// cv::Mat TRCV(3, 4, CV_32F);`	82	`// cv::Mat TRCV(3, 4, CV_32F);`
80	`// cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));`	83	`// cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));`
81	`// cv::Mat(calibration.Tr).copyTo(TRCV.col(3));`	84	`// cv::Mat(calibration.Tr).copyTo(TRCV.col(3));`
82	`// Eigen::Affine3f TR;`	85	`// Eigen::Affine3f TR;`
83	`// cv::cv2eigen(TRCV, TR.matrix());`	86	`// cv::cv2eigen(TRCV, TR.matrix());`
84	`// pcl::transformPointCloud(pointCloudPCL, pointCloudPCL, TR);`	87	`// pcl::transformPointCloud(pointCloudPCL, pointCloudPCL, TR);`
85		88
86	`// // Estimate surface normals (does not produce proper normals...)`	89	`// // Estimate surface normals (does not produce proper normals...)`
87	`// std::cout << "Estimating normals..." << std::endl;`	90	`// std::cout << "Estimating normals..." << std::endl;`
88	`// pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);`	91	`// pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);`
89	`// pcl::copyPointCloud(pointCloudPCL, points);`	92	`// pcl::copyPointCloud(pointCloudPCL, points);`
90	`// pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);`	93	`// pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);`
91	`// pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;`	94	`// pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;`
92	`// pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());`	95	`// pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());`
93	`// tree->setInputCloud(points);`	96	`// tree->setInputCloud(points);`
94	`// ne.setSearchMethod(tree);`	97	`// ne.setSearchMethod(tree);`
95	`// ne.setRadiusSearch(1.0);`	98	`// ne.setRadiusSearch(1.0);`
96	`// //ne.setKSearch(50);`	99	`// //ne.setKSearch(50);`
97	`// ne.setViewPoint(0.0, 0.0, 0.0);`	100	`// ne.setViewPoint(0.0, 0.0, 0.0);`
98	`// ne.setInputCloud(points);`	101	`// ne.setInputCloud(points);`
99	`// ne.compute(*normals);`	102	`// ne.compute(*normals);`
100	`// pcl::copyPointCloud(normals, pointCloudPCL);`	103	`// pcl::copyPointCloud(normals, pointCloudPCL);`
101		104
102	`// Assemble SMPointCloud data structure`	105	`// Assemble SMPointCloud data structure`
103	`SMPointCloud smPointCloud;`	106	`SMPointCloud smPointCloud;`
104	`smPointCloud.id = frameSequence.id;`	107	`smPointCloud.id = frameSequence.id;`
105	`smPointCloud.pointCloud = pointCloudPCL;`	108	`smPointCloud.pointCloud = pointCloudPCL;`
106	`smPointCloud.rotationAngle = frameSequence.rotationAngle;`	109	`smPointCloud.rotationAngle = frameSequence.rotationAngle;`
107		110
108	`// Determine transform in world (camera0) coordinate system`	111	`// Determine transform in world (camera0) coordinate system`
109	`float angleRadians = frameSequence.rotationAngle/180.0*M_PI;`	112	`float angleRadians = frameSequence.rotationAngle/180.0*M_PI;`
110	`cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`	113	`cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`
111	`cv::Mat R;`	114	`cv::Mat R;`
112	`cv::Rodrigues(rot_rvec, R);`	115	`cv::Rodrigues(rot_rvec, R);`
113	`smPointCloud.R = calibration.Rr.t()cv::Matx33f(R)calibration.Rr;`	116	`smPointCloud.R = calibration.Rr.t()cv::Matx33f(R)calibration.Rr;`
114	`smPointCloud.T = calibration.Rr.t()cv::Matx33f(R)calibration.Tr - calibration.Rr.t()*calibration.Tr;`	117	`smPointCloud.T = calibration.Rr.t()cv::Matx33f(R)calibration.Tr - calibration.Rr.t()*calibration.Tr;`
115		118
116		119
117	`// // Determine transform in world (camera0) coordinate system`	120	`// // Determine transform in world (camera0) coordinate system`
118	`// float angleRadians = frameSequence.rotationAngle/180.0*M_PI;`	121	`// float angleRadians = frameSequence.rotationAngle/180.0*M_PI;`
119	`// cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`	122	`// cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`
120	`// cv::Mat R;`	123	`// cv::Mat R;`
121	`// cv::Rodrigues(rot_rvec, R);`	124	`// cv::Rodrigues(rot_rvec, R);`
122	`// smPointCloud.R = cv::Matx33f(R);`	125	`// smPointCloud.R = cv::Matx33f(R);`
123	`// smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);`	126	`// smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);`
124		127
125	`// Emit result`	128	`// Emit result`
126	`emit newPointCloud(smPointCloud);`	129	`emit newPointCloud(smPointCloud);`
127		130
128	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`	131	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`
129	`}`	132	`}`
130		133
131	`void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){`	134	`void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){`
132		135
133	`// Process sequentially`	136	`// Process sequentially`
134	`for(unsigned int i=0; i<frameSequences.size(); i++){`	137	`for(unsigned int i=0; i<frameSequences.size(); i++){`
135	`reconstructPointCloud(frameSequences[i]);`	138	`reconstructPointCloud(frameSequences[i]);`
136	`}`	139	`}`
137		140
138	`}`	141	`}`
139		142
140	`void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){`	143	`void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){`
141		144
142	`cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));`	145	`cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));`
143	`cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));`	146	`cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));`
144		147
145	`cv::Mat temp(3,4,CV_32F);`	148	`cv::Mat temp(3,4,CV_32F);`
146	`cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`	149	`cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`
147	`cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`	150	`cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`
148	`cv::Mat P1 = cv::Mat(calibration.K1) * temp;`	151	`cv::Mat P1 = cv::Mat(calibration.K1) * temp;`
149		152
150	`cv::Mat QMatHomogenous, QMat;`	153	`cv::Mat QMatHomogenous, QMat;`
151	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`	154	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`
152	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`	155	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`
153	`cvtools::matToPoints3f(QMat, Q);`	156	`cvtools::matToPoints3f(QMat, Q);`
154		157
155		158
156	`}`	159	`}`
157		160