| Line 16... |
Line 16... |
| 16 |
#include <opencv2/core/eigen.hpp>
|
16 |
#include <opencv2/core/eigen.hpp>
|
| 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 |
#include <pcl/features/normal_3d_omp.h>
|
| 21 |
#include <pcl/common/transforms.h>
|
22 |
#include <pcl/common/transforms.h>
|
| 22 |
|
23 |
|
| 23 |
|
24 |
|
| 24 |
void SMReconstructionWorker::setup(){
|
25 |
void SMReconstructionWorker::setup(){
|
| 25 |
|
26 |
|
| Line 80... |
Line 81... |
| 80 |
// cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
|
81 |
// cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
|
| 81 |
// Eigen::Affine3f TR;
|
82 |
// Eigen::Affine3f TR;
|
| 82 |
// cv::cv2eigen(TRCV, TR.matrix());
|
83 |
// cv::cv2eigen(TRCV, TR.matrix());
|
| 83 |
// pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
|
84 |
// pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
|
| 84 |
|
85 |
|
| 85 |
// Estimate surface normals
|
86 |
// // Estimate surface normals (does not produce proper normals...)
|
| 86 |
std::cout << "Estimating normals..." << std::endl;
|
87 |
// std::cout << "Estimating normals..." << std::endl;
|
| - |
|
88 |
// pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
|
| 87 |
// This is much too slow to leave it on by default
|
89 |
// pcl::copyPointCloud(*pointCloudPCL, *points);
|
| - |
|
90 |
// pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
|
| 88 |
pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
|
91 |
// pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
|
| 89 |
// pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
92 |
// pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
|
| 90 |
// pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
|
93 |
// tree->setInputCloud(points);
|
| 91 |
ne.setKSearch(10);
|
94 |
// ne.setSearchMethod(tree);
|
| 92 |
//ne.setRadiusSearch(0.5);
|
95 |
// ne.setRadiusSearch(1.0);
|
| - |
|
96 |
// //ne.setKSearch(50);
|
| 93 |
ne.setViewPoint(0.0, 0.0, 0.0);
|
97 |
// ne.setViewPoint(0.0, 0.0, 0.0);
|
| 94 |
ne.setInputCloud(pointCloudPCL);
|
98 |
// ne.setInputCloud(points);
|
| 95 |
ne.compute(*pointCloudPCL);
|
99 |
// ne.compute(*normals);
|
| - |
|
100 |
// pcl::copyPointCloud(*normals, *pointCloudPCL);
|
| 96 |
|
101 |
|
| 97 |
// Assemble SMPointCloud data structure
|
102 |
// Assemble SMPointCloud data structure
|
| 98 |
SMPointCloud smPointCloud;
|
103 |
SMPointCloud smPointCloud;
|
| 99 |
smPointCloud.id = frameSequence.id;
|
104 |
smPointCloud.id = frameSequence.id;
|
| 100 |
smPointCloud.pointCloud = pointCloudPCL;
|
105 |
smPointCloud.pointCloud = pointCloudPCL;
|