Subversion Repositories seema-scanner

Rev

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

Rev 169 Rev 180
Line 12... Line 12...
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 
#include <opencv2/core/eigen.hpp>
17 
 
 18 
 
18 
#include <pcl/filters/statistical_outlier_removal.h>
 19 
#include <pcl/filters/statistical_outlier_removal.h>
19 
#include <pcl/io/pcd_io.h>
 20 
#include <pcl/io/pcd_io.h>
20 
#include <pcl/features/normal_3d.h>
 21 
#include <pcl/features/normal_3d.h>
- 
 
 22 
#include <pcl/common/transforms.h>
21 
 
 23 
 
22 
 
 24 
 
23 
void SMReconstructionWorker::setup(){
 25 
void SMReconstructionWorker::setup(){
24 
 
 26 
 
25 
 
 27 
 
Line 73... Line 75...
73 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
 75 
        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];
 76 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
75 
        pointCloudPCL->points[i] = point;
 77 
        pointCloudPCL->points[i] = point;
76 
    }
 78 
    }
77 
 
 79 
 
- 
 
 80 
 
- 
 
 81 
 
- 
 
 82 
 
- 
 
 83 
 
- 
 
 84 
//    // Transform point cloud to rotation axis coordinate system
- 
 
 85 
//    cv::Mat TRCV(3, 4, CV_32F);
- 
 
 86 
//    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
- 
 
 87 
//    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
- 
 
 88 
//    Eigen::Affine3f TR;
- 
 
 89 
//    cv::cv2eigen(TRCV, TR.matrix());
- 
 
 90 
//    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
- 
 
 91 
 
- 
 
 92 
 
- 
 
 93 
 
- 
 
 94 
 
78 
//    // Estimate surface normals
 95 
//    // Estimate surface normals
79 
    // This is much to slow to leave it on by default
 96 
    // This is much to slow to leave it on by default
80 
//    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
 97 
//    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
81 
//    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 98 
//    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
82 
//    pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
 99 
//    pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
Line 98... Line 115...
98 
    cv::Mat R;
 115 
    cv::Mat R;
99 
    cv::Rodrigues(rot_rvec, R);
 116 
    cv::Rodrigues(rot_rvec, R);
100 
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
 117 
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
101 
    smPointCloud.T = calibration.Rr.t()*cv::Matx33f(R)*calibration.Tr - calibration.Rr.t()*calibration.Tr;
 118 
    smPointCloud.T = calibration.Rr.t()*cv::Matx33f(R)*calibration.Tr - calibration.Rr.t()*calibration.Tr;
102 
 
 119 
 
- 
 
 120 
 
- 
 
 121 
//    // Determine transform in world (camera0) coordinate system
- 
 
 122 
//    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
- 
 
 123 
//    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
- 
 
 124 
//    cv::Mat R;
- 
 
 125 
//    cv::Rodrigues(rot_rvec, R);
- 
 
 126 
//    smPointCloud.R = cv::Matx33f(R);
- 
 
 127 
//    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);
- 
 
 128 
 
- 
 
 129 
 
- 
 
 130 
 
- 
 
 131 
 
103 
    // Emit result
 132 
    // Emit result
104 
    emit newPointCloud(smPointCloud);
 133 
    emit newPointCloud(smPointCloud);
105 
 
 134 
 
106 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 135 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
107 
 
 136