Subversion Repositories seema-scanner

Rev

Rev 42 | Rev 45 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 42 Rev 44
Line 11... Line 11...
11 
 
 11 
 
12 
#include "cvtools.h"
 12 
#include "cvtools.h"
13 
 
 13 
 
14 
#include <pcl/filters/statistical_outlier_removal.h>
 14 
#include <pcl/filters/statistical_outlier_removal.h>
15 
#include <pcl/io/pcd_io.h>
 15 
#include <pcl/io/pcd_io.h>
- 
 
 16 
#include <pcl/features/normal_3d.h>
16 
 
 17 
 
17 
 
 18 
 
18 
void SMReconstructionWorker::setup(){
 19 
void SMReconstructionWorker::setup(){
19 
 
 20 
 
20 
    QSettings settings;
 21 
    QSettings settings;
Line 58... Line 59...
58 
    std::vector<cv::Point3f> Q;
 59 
    std::vector<cv::Point3f> Q;
59 
    std::vector<cv::Vec3b> color;
 60 
    std::vector<cv::Vec3b> color;
60 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
 61 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
61 
 
 62 
 
62 
    // Convert point cloud to PCL format
 63 
    // Convert point cloud to PCL format
63 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
 64 
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
64 
 
 65 
 
65 
    // Interpret as unorganized point cloud
 - 
 
66 
    pointCloudPCL->width = Q.size();
 66 
    pointCloudPCL->width = Q.size();
67 
    pointCloudPCL->height = 1;
 67 
    pointCloudPCL->height = 1;
68 
    pointCloudPCL->is_dense = false;
 68 
    pointCloudPCL->is_dense = false;
69 
 
 69 
 
70 
    pointCloudPCL->points.resize(Q.size());
 70 
    pointCloudPCL->points.resize(Q.size());
71 
 
 71 
 
72 
    for(int i=0; i<Q.size(); i++){
 72 
    for(int i=0; i<Q.size(); i++){
73 
        pcl::PointXYZRGB point;
 73 
        pcl::PointXYZRGBNormal point;
74 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
 74 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
75 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
 75 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
76 
        pointCloudPCL->points[i] = point;
 76 
        pointCloudPCL->points[i] = point;
77 
    }
 77 
    }
78 
 
 78 
 
- 
 
 79 
    // Estimate surface normals
- 
 
 80 
    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
- 
 
 81 
    pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
- 
 
 82 
    ne.setSearchMethod(tree);
- 
 
 83 
    ne.setRadiusSearch(3);
- 
 
 84 
    ne.setViewPoint(0.0, 0.0, 0.0);
- 
 
 85 
    ne.setInputCloud(pointCloudPCL);
- 
 
 86 
    ne.compute(*pointCloudPCL);
- 
 
 87 
 
- 
 
 88 
    // Assemble SMPointCloud data structure
79 
    SMPointCloud smPointCloud;
 89 
    SMPointCloud smPointCloud;
80 
    smPointCloud.pointCloud = pointCloudPCL;
 90 
    smPointCloud.pointCloud = pointCloudPCL;
81 
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
 91 
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
82 
 
 92 
 
- 
 
 93 
    // Determine transform in world (camera0) coordinate system
- 
 
 94 
    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
- 
 
 95 
    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
- 
 
 96 
    cv::Mat R;
- 
 
 97 
    cv::Rodrigues(rot_rvec, R);
- 
 
 98 
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
- 
 
 99 
    smPointCloud.T = calibration.Rr.t()*cv::Matx33f(R)*calibration.Tr - calibration.Rr.t()*calibration.Tr;
- 
 
 100 
 
83 
    // Emit result
 101 
    // Emit result
84 
    emit newPointCloud(smPointCloud);
 102 
    emit newPointCloud(smPointCloud);
85 
 
 103 
 
86 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 104 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
87 
 
 105