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


#include "SMReconstructionWorker.h"
 
#include "AlgorithmGrayCode.h"
#include "AlgorithmGrayCodeHorzVert.h"
#include "AlgorithmGrayCodeMax.h"
#include "AlgorithmPhaseShiftTwoFreq.h"
#include "AlgorithmPhaseShiftThreeFreq.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/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 == "GrayCodeMax")
        algorithm = new AlgorithmGrayCodeMax(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;
    }
 
 
 
 
 
//    // 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
    // This is much to slow to leave it on by default
//    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;
 
 
//    // 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 169 → 180

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