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 <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 == "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;
    }
 
//    // 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;
 
    // 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 167 → 169

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