Subversion Repositories seema-scanner

Rev

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

Rev 182 Rev 183
Line 72... Line 72...
72 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
 72 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
73 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
 73 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
74 
        pointCloudPCL->points[i] = point;
 74 
        pointCloudPCL->points[i] = point;
75 
    }
 75 
    }
76 
 
 76 
 
77 
 
 - 
 
78 
 
 - 
 
79 
 
 - 
 
80 
 
 - 
 
81 
//    // Transform point cloud to rotation axis coordinate system
 77 
//    // Transform point cloud to rotation axis coordinate system
82 
//    cv::Mat TRCV(3, 4, CV_32F);
 78 
//    cv::Mat TRCV(3, 4, CV_32F);
83 
//    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
 79 
//    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
84 
//    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
 80 
//    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
85 
//    Eigen::Affine3f TR;
 81 
//    Eigen::Affine3f TR;
86 
//    cv::cv2eigen(TRCV, TR.matrix());
 82 
//    cv::cv2eigen(TRCV, TR.matrix());
87 
//    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
 83 
//    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
88 
 
 84 
 
89 
 
 - 
 
90 
 
 - 
 
91 
 
 - 
 
92 
//    // Estimate surface normals
 85 
    // Estimate surface normals
- 
 
 86 
    std::cout << "Estimating normals..." << std::endl;
93 
    // This is much to slow to leave it on by default
 87 
    // This is much too slow to leave it on by default
94 
//    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
 88 
    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
95 
//    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 89 
//    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCLCopy(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
96 
//    pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
 90 
//    pcl::copyPointCloud(*pointCloudPCL, *pointCloudPCLCopy);
97 
//    //ne.setKSearch(10);
 91 
    ne.setKSearch(10);
98 
//    ne.setRadiusSearch(0.5);
 92 
    //ne.setRadiusSearch(0.5);
99 
//    ne.setViewPoint(0.0, 0.0, 0.0);
 93 
    ne.setViewPoint(0.0, 0.0, 0.0);
100 
//    ne.setInputCloud(pointCloudPCLCopy);
 94 
    ne.setInputCloud(pointCloudPCL);
101 
//    ne.compute(*pointCloudPCL);
 95 
    ne.compute(*pointCloudPCL);
102 
 
 96 
 
103 
    // Assemble SMPointCloud data structure
 97 
    // Assemble SMPointCloud data structure
104 
    SMPointCloud smPointCloud;
 98 
    SMPointCloud smPointCloud;
105 
    smPointCloud.id = frameSequence.id;
 99 
    smPointCloud.id = frameSequence.id;
106 
    smPointCloud.pointCloud = pointCloudPCL;
 100 
    smPointCloud.pointCloud = pointCloudPCL;
Line 121... Line 115...
121 
//    cv::Mat R;
 115 
//    cv::Mat R;
122 
//    cv::Rodrigues(rot_rvec, R);
 116 
//    cv::Rodrigues(rot_rvec, R);
123 
//    smPointCloud.R = cv::Matx33f(R);
 117 
//    smPointCloud.R = cv::Matx33f(R);
124 
//    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);
 118 
//    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);
125 
 
 119 
 
126 
 
 - 
 
127 
 
 - 
 
128 
 
 - 
 
129 
    // Emit result
 120 
    // Emit result
130 
    emit newPointCloud(smPointCloud);
 121 
    emit newPointCloud(smPointCloud);
131 
 
 122 
 
132 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 123 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
133 
 
 - 
 
134 
}
 124 
}
135 
 
 125 
 
136 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
 126 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
137 
 
 127 
 
138 
    // Process sequentially
 128 
    // Process sequentially