Subversion Repositories seema-scanner

Rev

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

Rev 200 Rev 207
Line 23... Line 23...
23 
#include <pcl/features/normal_3d_omp.h>
 23 
#include <pcl/features/normal_3d_omp.h>
24 
#include <pcl/common/transforms.h>
 24 
#include <pcl/common/transforms.h>
25 
 
 25 
 
26 
 
 26 
 
27 
void SMReconstructionWorker::setup(){
 27 
void SMReconstructionWorker::setup(){
28 
 
 - 
 
29 
 
 - 
 
30 
}
 28 
}
31 
 
 29 
 
32 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
 30 
void SMReconstructionWorker::reconstructPointCloud(const SMFrameSequence &frameSequence){
33 
 
 31 
 
34 
    QSettings settings;
 32 
    QSettings settings;
35 
 
 33 
 
36 
    // Get current calibration
 34 
    // Get current calibration
37 
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 35 
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
Line 79... Line 77...
79 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
 77 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
80 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
 78 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
81 
        pointCloudPCL->points[i] = point;
 79 
        pointCloudPCL->points[i] = point;
82 
    }
 80 
    }
83 
 
 81 
 
84 
//    // Transform point cloud to rotation axis coordinate system
 82 
    // Transform point cloud to rotation axis coordinate system
85 
//    cv::Mat TRCV(3, 4, CV_32F);
 83 
    /*cv::Mat TRCV(3, 4, CV_32F);
86 
//    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
 84 
    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
87 
//    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
 85 
    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
88 
//    Eigen::Affine3f TR;
 86 
    Eigen::Affine3f TR;
89 
//    cv::cv2eigen(TRCV, TR.matrix());
 87 
    cv::cv2eigen(TRCV, TR.matrix());
90 
//    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
 88 
    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
91 
 
 89 
 
92 
//    // Estimate surface normals (does not produce proper normals...)
 90 
    // Estimate surface normals (does not produce proper normals...)
93 
//    std::cout << "Estimating normals..." << std::endl;
 91 
    std::cout << "Estimating normals..." << std::endl;
94 
//    pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
 92 
    pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
95 
//    pcl::copyPointCloud(*pointCloudPCL, *points);
 93 
    pcl::copyPointCloud(*pointCloudPCL, *points);
96 
//    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
 94 
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
97 
//    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
 95 
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
98 
//    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
 96 
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
99 
//    tree->setInputCloud(points);
 97 
    tree->setInputCloud(points);
100 
//    ne.setSearchMethod(tree);
 98 
    ne.setSearchMethod(tree);
101 
//    ne.setRadiusSearch(1.0);
 99 
    ne.setRadiusSearch(1.0);
102 
//    //ne.setKSearch(50);
 100 
    //ne.setKSearch(50);
103 
//    ne.setViewPoint(0.0, 0.0, 0.0);
 101 
    ne.setViewPoint(0.0, 0.0, 0.0);
104 
//    ne.setInputCloud(points);
 102 
    ne.setInputCloud(points);
105 
//    ne.compute(*normals);
 103 
    ne.compute(*normals);
106 
//    pcl::copyPointCloud(*normals, *pointCloudPCL);
 104 
    pcl::copyPointCloud(*normals, *pointCloudPCL);*/
107 
 
 105 
 
108 
    // Assemble SMPointCloud data structure
 106 
    // Assemble SMPointCloud data structure
109 
    SMPointCloud smPointCloud;
 107 
    SMPointCloud smPointCloud;
110 
    smPointCloud.id = frameSequence.id;
 108 
    smPointCloud.id = frameSequence.id;
111 
    smPointCloud.pointCloud = pointCloudPCL;
 109 
    smPointCloud.pointCloud = pointCloudPCL;
Line 118... Line 116...
118 
    cv::Rodrigues(rot_rvec, R);
 116 
    cv::Rodrigues(rot_rvec, R);
119 
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
 117 
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
120 
    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;
121 
 
 119 
 
122 
 
 120 
 
123 
//    // Determine transform in world (camera0) coordinate system
 121 
    // Determine transform in world (camera0) coordinate system
124 
//    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
 122 
    /*float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
125 
//    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
 123 
    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
126 
//    cv::Mat R;
 124 
    cv::Mat R;
127 
//    cv::Rodrigues(rot_rvec, R);
 125 
    cv::Rodrigues(rot_rvec, R);
128 
//    smPointCloud.R = cv::Matx33f(R);
 126 
    smPointCloud.R = cv::Matx33f(R);
129 
//    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);
 127 
    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);*/
130 
 
 128 
 
131 
    // Emit result
 129 
    // Emit result
- 
 
 130 
    #pragma omp critical (CBRECOupdateUI1)
- 
 
 131 
    {
132 
    emit newPointCloud(smPointCloud);
 132 
        emit newPointCloud(smPointCloud);
133 
 
 - 
 
134 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 133 
        std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
- 
 
 134 
    }
135 
}
 135 
}
136 
 
 136 
 
137 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
 137 
void SMReconstructionWorker::reconstructPointClouds(const std::vector<SMFrameSequence> &frameSequences){
138 
 
 - 
 
139 
    // Process sequentially
 138 
    // Process sequentially
- 
 
 139 
    #pragma omp parallel for
140 
    for(unsigned int i=0; i<frameSequences.size(); i++){
 140 
    for(unsigned int i=0; i<frameSequences.size(); i++){
141 
        reconstructPointCloud(frameSequences[i]);
 141 
        if(!frameSequences[i].reconstructed) reconstructPointCloud(frameSequences[i]);
142 
    }
 142 
    }
143 
 
 - 
 
144 
}
 143 
}
145 
 
 144 
 
146 
void SMReconstructionWorker::triangulate(std::vector<cv::Point2f>& q0, std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){
 145 
void SMReconstructionWorker::triangulate(const std::vector<cv::Point2f>& q0, const std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){
147 
 
 - 
 
148 
    cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));
 146 
    cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));
149 
    cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));
 147 
    cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));
150 
 
 148 
 
151 
    cv::Mat temp(3,4,CV_32F);
 149 
    cv::Mat temp(3,4,CV_32F);
152 
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
 150 
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
Line 155... Line 153...
155 
 
 153 
 
156 
    cv::Mat QMatHomogenous, QMat;
 154 
    cv::Mat QMatHomogenous, QMat;
157 
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
 155 
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
158 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 156 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
159 
    cvtools::matToPoints3f(QMat, Q);
 157 
    cvtools::matToPoints3f(QMat, Q);
160 
 
 - 
 
161 
 
 - 
 
162 
}
 158 
}