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 <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 == "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 180 → 182

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