Subversion Repositories seema-scanner

Rev

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

Rev 41 Rev 42
Line 7... Line 7...
7 
#include <QSettings>
 7 
#include <QSettings>
8 
 
 8 
 
9 
#include <iostream>
 9 
#include <iostream>
10 
#include <opencv2/opencv.hpp>
 10 
#include <opencv2/opencv.hpp>
11 
 
 11 
 
- 
 
 12 
#include "cvtools.h"
- 
 
 13 
 
12 
#include <pcl/filters/statistical_outlier_removal.h>
 14 
#include <pcl/filters/statistical_outlier_removal.h>
13 
#include <pcl/io/pcd_io.h>
 15 
#include <pcl/io/pcd_io.h>
14 
 
 16 
 
15 
 
 17 
 
16 
void SMReconstructionWorker::setup(){
 18 
void SMReconstructionWorker::setup(){
Line 34... Line 36...
34 
        algorithm = new AlgorithmGrayCode(resX, resY, dir);
 36 
        algorithm = new AlgorithmGrayCode(resX, resY, dir);
35 
    else
 37 
    else
36 
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
 38 
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
37 
 
 39 
 
38 
 
 40 
 
39 
    // Precompute lens correction maps
 41 
//    // Precompute lens correction maps
40 
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
 42 
//    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
41 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);
 43 
//    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);
42 
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);
 44 
//    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);
43 
 
 45 
 
44 
    //cv::Mat mapHorz, mapVert;
 46 
    //cv::Mat mapHorz, mapVert;
45 
    //cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);
 47 
    //cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);
46 
    //cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);
 48 
    //cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);
47 
    //cv::imwrite("mapHorz.png", mapHorz);
 49 
    //cv::imwrite("mapHorz.png", mapHorz);
Line 50... Line 52...
50 
 
 52 
 
51 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
 53 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
52 
 
 54 
 
53 
    time.start();
 55 
    time.start();
54 
 
 56 
 
55 
    // Get correspondences
 57 
    // Get 3D Points
56 
    std::vector<cv::Point2f> q0, q1;
 - 
 
57 
    std::vector<cv::Point3f> color;
 - 
 
58 
    algorithm->getCorrespondences(calibration, frameSequence.frames0, frameSequence.frames0, q0, q1, color);
 - 
 
59 
 
 - 
 
60 
    // Triangulate
 - 
 
61 
    std::vector<cv::Point3f> Q;
 58 
    std::vector<cv::Point3f> Q;
62 
    triangulate(q0, q1, Q);
 59 
    std::vector<cv::Vec3b> color;
- 
 
 60 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
63 
 
 61 
 
64 
    // Convert point cloud to PCL format
 62 
    // Convert point cloud to PCL format
65 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
 63 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
66 
 
 64 
 
67 
    // Interprete as organized point cloud
 65 
    // Interpret as unorganized point cloud
68 
    pointCloudPCL->width = Q.size();
 66 
    pointCloudPCL->width = Q.size();
69 
    pointCloudPCL->height = 1;
 67 
    pointCloudPCL->height = 1;
70 
    pointCloudPCL->is_dense = false;
 68 
    pointCloudPCL->is_dense = false;
71 
 
 69 
 
72 
    pointCloudPCL->points.resize(Q.size());
 70 
    pointCloudPCL->points.resize(Q.size());
73 
 
 71 
 
74 
    for(int i=0; i<Q.size(); i++){
 72 
    for(int i=0; i<Q.size(); i++){
75 
        pcl::PointXYZRGB point;
 73 
        pcl::PointXYZRGB point;
76 
        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;
77 
        point.r = color[i].x; point.g = color[i].y; point.b = color[i].z;
 75 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
78 
        pointCloudPCL->points[i] = point;
 76 
        pointCloudPCL->points[i] = point;
79 
    }
 77 
    }
80 
 
 78 
 
- 
 
 79 
    SMPointCloud smPointCloud;
- 
 
 80 
    smPointCloud.pointCloud = pointCloudPCL;
- 
 
 81 
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
- 
 
 82 
 
81 
    // Emit result
 83 
    // Emit result
82 
    emit newPointCloud(pointCloudPCL);
 84 
    emit newPointCloud(smPointCloud);
83 
 
 85 
 
84 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 86 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
85 
 
 87 
 
86 
}
 88 
}
87 
 
 89 
 
Line 102... Line 104...
102 
    cv::Mat temp(3,4,CV_32F);
 104 
    cv::Mat temp(3,4,CV_32F);
103 
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
 105 
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
104 
    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
 106 
    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
105 
    cv::Mat P1 = cv::Mat(calibration.K1) * temp;
 107 
    cv::Mat P1 = cv::Mat(calibration.K1) * temp;
106 
 
 108 
 
- 
 
 109 
    cv::Mat QMatHomogenous, QMat;
107 
    cv::triangulatePoints(P0, P1, q0, q1, Q);
 110 
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
- 
 
 111 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
- 
 
 112 
    cvtools::matToPoints3f(QMat, Q);
- 
 
 113 
 
108 
 
 114 
 
109 
}
 115 
}
110 
 
 116 
 
111 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 117 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
112 
 
 118