| Line 1... |
Line 1... |
| 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 "AlgorithmPhaseShift.h"
|
5 |
#include "AlgorithmPhaseShiftTwoFreq.h"
|
| - |
|
6 |
#include "AlgorithmPhaseShiftThreeFreq.h"
|
| 6 |
#include "AlgorithmLineShift.h"
|
7 |
#include "AlgorithmLineShift.h"
|
| 7 |
|
8 |
|
| 8 |
#include <QCoreApplication>
|
9 |
#include <QCoreApplication>
|
| 9 |
#include <QSettings>
|
10 |
#include <QSettings>
|
| 10 |
|
11 |
|
| Line 31... |
Line 32... |
| 31 |
QString codec = settings.value("algorithm", "GrayCode").toString();
|
32 |
QString codec = settings.value("algorithm", "GrayCode").toString();
|
| 32 |
if(codec == "GrayCode")
|
33 |
if(codec == "GrayCode")
|
| 33 |
algorithm = new AlgorithmGrayCode(resX, resY);
|
34 |
algorithm = new AlgorithmGrayCode(resX, resY);
|
| 34 |
else if(codec == "GrayCodeHorzVert")
|
35 |
else if(codec == "GrayCodeHorzVert")
|
| 35 |
algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);
|
36 |
algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);
|
| 36 |
else if(codec == "PhaseShift")
|
37 |
else if(codec == "PhaseShiftTwoFreq")
|
| 37 |
algorithm = new AlgorithmPhaseShift(resX, resY);
|
38 |
algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);
|
| - |
|
39 |
else if(codec == "PhaseShiftThreeFreq")
|
| - |
|
40 |
algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);
|
| 38 |
else if(codec == "LineShift")
|
41 |
else if(codec == "LineShift")
|
| 39 |
algorithm = new AlgorithmLineShift(resX, resY);
|
42 |
algorithm = new AlgorithmLineShift(resX, resY);
|
| 40 |
else
|
43 |
else
|
| 41 |
std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
|
44 |
std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
|
| 42 |
|
45 |
|
| Line 61... |
Line 64... |
| 61 |
std::vector<cv::Point3f> Q;
|
64 |
std::vector<cv::Point3f> Q;
|
| 62 |
std::vector<cv::Vec3b> color;
|
65 |
std::vector<cv::Vec3b> color;
|
| 63 |
algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
|
66 |
algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
|
| 64 |
|
67 |
|
| 65 |
// Convert point cloud to PCL format
|
68 |
// Convert point cloud to PCL format
|
| 66 |
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
|
69 |
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
| 67 |
|
70 |
|
| 68 |
pointCloudPCL->width = Q.size();
|
71 |
pointCloudPCL->width = Q.size();
|
| 69 |
pointCloudPCL->height = 1;
|
72 |
pointCloudPCL->height = 1;
|
| 70 |
pointCloudPCL->is_dense = false;
|
73 |
pointCloudPCL->is_dense = true;
|
| 71 |
|
74 |
|
| 72 |
pointCloudPCL->points.resize(Q.size());
|
75 |
pointCloudPCL->points.resize(Q.size());
|
| 73 |
|
76 |
|
| 74 |
for(unsigned int i=0; i<Q.size(); i++){
|
77 |
for(unsigned int i=0; i<Q.size(); i++){
|
| 75 |
pcl::PointXYZRGB point;
|
78 |
pcl::PointXYZRGBNormal point;
|
| 76 |
point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
|
79 |
point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
|
| 77 |
point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
|
80 |
point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
|
| 78 |
pointCloudPCL->points[i] = point;
|
81 |
pointCloudPCL->points[i] = point;
|
| 79 |
}
|
82 |
}
|
| 80 |
|
83 |
|
| 81 |
// // Estimate surface normals
|
84 |
// Estimate surface normals
|
| 82 |
// pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
|
85 |
pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
|
| 83 |
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
|
86 |
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
| - |
|
87 |
pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
|
| 84 |
// ne.setSearchMethod(tree);
|
88 |
//ne.setKSearch(10);
|
| 85 |
// ne.setRadiusSearch(3);
|
89 |
ne.setRadiusSearch(0.5);
|
| 86 |
// ne.setViewPoint(0.0, 0.0, 0.0);
|
90 |
ne.setViewPoint(0.0, 0.0, 0.0);
|
| 87 |
// ne.setInputCloud(pointCloudPCL);
|
91 |
ne.setInputCloud(pointCloudPCLCopy);
|
| 88 |
// ne.compute(*pointCloudPCL);
|
92 |
ne.compute(*pointCloudPCL);
|
| 89 |
|
93 |
|
| 90 |
// Assemble SMPointCloud data structure
|
94 |
// Assemble SMPointCloud data structure
|
| 91 |
SMPointCloud smPointCloud;
|
95 |
SMPointCloud smPointCloud;
|
| 92 |
smPointCloud.id = frameSequence.id;
|
96 |
smPointCloud.id = frameSequence.id;
|
| 93 |
smPointCloud.pointCloud = pointCloudPCL;
|
97 |
smPointCloud.pointCloud = pointCloudPCL;
|