Subversion Repositories seema-scanner

Rev

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

Rev 9 Rev 24
Line 1... Line 1...
1 
#include "SMTriangulationWorker.h"
 1 
#include "SMSMTriangulator.h"
2 
 
 2 
 
3 
#include <QCoreApplication>
 3 
#include <QCoreApplication>
4 
#include <QSettings>
 4 
#include <QSettings>
5 
 
 5 
 
6 
#include <iostream>
 6 
#include <iostream>
7 
#include <opencv2/opencv.hpp>
 7 
#include <opencv2/opencv.hpp>
8 
 
 8 
 
9 
#include <pcl/filters/statistical_outlier_removal.h>
 9 
#include <pcl/filters/statistical_outlier_removal.h>
10 
#include <pcl/io/pcd_io.h>
 10 
#include <pcl/io/pcd_io.h>
11 
 
 11 
 
12 
void SMTriangulationWorker::setup(){
 12 
void SMSMTriangulator::setup(){
13 
 
 13 
 
14 
    // Initialize triangulator with calibration
 14 
    // Initialize SMTriangulator with calibration
15 
    calibration = new SMCalibrationParams;
 15 
    calibration = new SMCalibrationParams;
16 
    calibration->load("calibration.yml");
 16 
    calibration->load("calibration.yml");
17 
    triangulator = new Triangulator(*calibration, frameWidth, frameHeight);
 17 
    SMTriangulator = new SMTriangulator(*calibration, frameWidth, frameHeight);
18 
 
 18 
 
19 
    QSettings settings("SLStudio");
 19 
    QSettings settings("SLStudio");
20 
    writeToDisk = settings.value("writeToDisk/pointclouds",false).toBool();
 20 
    writeToDisk = settings.value("writeToDisk/pointclouds",false).toBool();
21 
 
 21 
 
22 
}
 22 
}
23 
 
 23 
 
24 
 
 24 
 
25 
void SMTriangulationWorker::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
 25 
void SMSMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
26 
 
 26 
 
27 
    time.start();
 27 
    time.start();
28 
 
 28 
 
29 
    // Reconstruct point cloud
 29 
    // Reconstruct point cloud
30 
    cv::Mat pointCloud;
 30 
    cv::Mat pointCloud;
31 
    cv::Mat empty;
 31 
    cv::Mat empty;
32 
    triangulator->triangulate(up, vp, mask, shading, pointCloud);
 32 
    SMTriangulator->triangulate(up, vp, mask, shading, pointCloud);
33 
 
 33 
 
34 
    std::vector<cv::Mat> xyz;
 34 
    std::vector<cv::Mat> xyz;
35 
    cv::split(pointCloud, xyz);
 35 
    cv::split(pointCloud, xyz);
36 
//    emit imshow("x", xyz[0], 1400, 100);
 36 
//    emit imshow("x", xyz[0], 1400, 100);
37 
//    emit imshow("y", xyz[1], 1400, 450);
 37 
//    emit imshow("y", xyz[1], 1400, 450);
Line 102... Line 102...
102 
//    filter.filter(*pointCloudFiltered);
 102 
//    filter.filter(*pointCloudFiltered);
103 
 
 103 
 
104 
    // Emit result
 104 
    // Emit result
105 
    emit newPointCloud(pointCloudPCL);
 105 
    emit newPointCloud(pointCloudPCL);
106 
 
 106 
 
107 
    std::cout << "Triangulator: " << time.elapsed() << "ms" << std::endl;
 107 
    std::cout << "SMTriangulator: " << time.elapsed() << "ms" << std::endl;
108 
 
 108 
 
109 
    if(writeToDisk){
 109 
    if(writeToDisk){
110 
        QString fileName = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmsszzz");
 110 
        QString fileName = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmsszzz");
111 
        fileName.append(".pcd");
 111 
        fileName.append(".pcd");
112 
        pcl::io::savePCDFileBinary(fileName.toStdString(), *pointCloudPCL);
 112 
        pcl::io::savePCDFileBinary(fileName.toStdString(), *pointCloudPCL);
113 
    }
 113 
    }
114 
 
 114 
 
115 
    //emit finished();
 115 
    //emit finished();
116 
}
 116 
}
117 
 
 117 
 
- 
 
 118 
 
- 
 
 119 
SMTriangulator::SMTriangulator(SMCalibrationParams _calibration, unsigned int frameWidth, unsigned int frameHeight) : calibration(_calibration){
- 
 
 120 
 
- 
 
 121 
    // Precompute uc, vc maps
- 
 
 122 
    uc.create(frameHeight, frameWidth, CV_32F);
- 
 
 123 
    vc.create(frameHeight, frameWidth, CV_32F);
- 
 
 124 
 
- 
 
 125 
    for(unsigned int row=0; row<frameHeight; row++){
- 
 
 126 
        for(unsigned int col=0; col<frameWidth; col++){
- 
 
 127 
            uc.at<float>(row, col) = col;
- 
 
 128 
            vc.at<float>(row, col) = row;
- 
 
 129 
        }
- 
 
 130 
    }
- 
 
 131 
 
- 
 
 132 
    // Precompute determinant tensor
- 
 
 133 
    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
- 
 
 134 
    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
- 
 
 135 
 
- 
 
 136 
    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
- 
 
 137 
    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
- 
 
 138 
    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
- 
 
 139 
    Pp = cv::Mat(calibration.Kp) * temp;
- 
 
 140 
 
- 
 
 141 
    cv::Mat e = cv::Mat::eye(4, 4, CV_32F);
- 
 
 142 
 
- 
 
 143 
    int sz[] = {4, 3, 3, 3};
- 
 
 144 
    cv::Mat C(4, sz, CV_32F, cv::Scalar::all(0));
- 
 
 145 
    for(int k=0; k<4; k++){
- 
 
 146 
        for(int i=0; i<3; i++){
- 
 
 147 
            for(int j=0; j<3; j++){
- 
 
 148 
                for(int l=0; l<3; l++){
- 
 
 149 
                    cv::Mat op(4, 4, CV_32F);
- 
 
 150 
                    Pc.row(i).copyTo(op.row(0));
- 
 
 151 
                    Pc.row(j).copyTo(op.row(1));
- 
 
 152 
                    Pp.row(l).copyTo(op.row(2));
- 
 
 153 
                    e.row(k).copyTo(op.row(3));
- 
 
 154 
                    C.at<float>(cv::Vec4i(k,i,j,l)) = cv::determinant(op.t());
- 
 
 155 
                }
- 
 
 156 
            }
- 
 
 157 
        }
- 
 
 158 
    }
- 
 
 159 
    determinantTensor = C;
- 
 
 160 
 
- 
 
 161 
    // Precompute lens correction maps
- 
 
 162 
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
- 
 
 163 
    cv::initUndistortRectifyMap(calibration.Kc, calibration.kc, eye, calibration.Kc, cv::Size(frameWidth, frameHeight), CV_32FC1, lensMap1, lensMap2);
- 
 
 164 
 
- 
 
 165 
    //cv::Mat map1, map2;
- 
 
 166 
    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
- 
 
 167 
    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
- 
 
 168 
    //cv::imwrite("map1.png", map1);
- 
 
 169 
    //cv::imwrite("map2.png", map2);
- 
 
 170 
}
- 
 
 171 
 
- 
 
 172 
void SMTriangulator::triangulate(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading, cv::Mat &pointCloud){
- 
 
 173 
 
- 
 
 174 
    // Undistort up, mask and shading
- 
 
 175 
    if(!up.empty()){
- 
 
 176 
        cv::Mat upUndistort;
- 
 
 177 
        cv::remap(up, upUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
- 
 
 178 
        up = upUndistort;
- 
 
 179 
    }
- 
 
 180 
    if(!vp.empty()){
- 
 
 181 
        cv::Mat vpUndistort;
- 
 
 182 
        cv::remap(vp, vpUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
- 
 
 183 
        vp = vpUndistort;
- 
 
 184 
    }
- 
 
 185 
 
- 
 
 186 
    cv::Mat maskUndistort, shadingUndistort;
- 
 
 187 
    cv::remap(mask, maskUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
- 
 
 188 
    cv::remap(shading, shadingUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
- 
 
 189 
    mask = maskUndistort;
- 
 
 190 
    shading = shadingUndistort;
- 
 
 191 
 
- 
 
 192 
    // Triangulate
- 
 
 193 
    cv::Mat xyz;
- 
 
 194 
    if(!up.empty() && vp.empty())
- 
 
 195 
        triangulateFromUp(up, xyz);
- 
 
 196 
    else if(!vp.empty() && up.empty())
- 
 
 197 
        triangulateFromVp(vp, xyz);
- 
 
 198 
    else if(!up.empty() && !vp.empty())
- 
 
 199 
        triangulateFromUpVp(up, vp, xyz);
- 
 
 200 
 
- 
 
 201 
    // Merge and mask
- 
 
 202 
    pointCloud = cv::Mat(up.size(), CV_32FC3, cv::Scalar(NAN, NAN, NAN));
- 
 
 203 
    xyz.copyTo(pointCloud, mask);
- 
 
 204 
 
- 
 
 205 
}
- 
 
 206 
 
- 
 
 207 
void SMTriangulator::triangulateFromUp(cv::Mat &up, cv::Mat &xyz){
- 
 
 208 
 
- 
 
 209 
    // Solve for xyzw using determinant tensor
- 
 
 210 
    cv::Mat C = determinantTensor;
- 
 
 211 
    std::vector<cv::Mat> xyzw(4);
- 
 
 212 
    for(unsigned int i=0; i<4; i++){
- 
 
 213 
        xyzw[i].create(up.size(), CV_32F);
- 
 
 214 
        xyzw[i] = C.at<float>(cv::Vec4i(i,0,1,0)) - C.at<float>(cv::Vec4i(i,2,1,0))*uc - C.at<float>(cv::Vec4i(i,0,2,0))*vc -
- 
 
 215 
                C.at<float>(cv::Vec4i(i,0,1,2))*up + C.at<float>(cv::Vec4i(i,2,1,2))*up.mul(uc) + C.at<float>(cv::Vec4i(i,0,2,2))*up.mul(vc);
- 
 
 216 
    }
- 
 
 217 
 
- 
 
 218 
    // Convert to non homogenous coordinates
- 
 
 219 
    for(unsigned int i=0; i<3; i++)
- 
 
 220 
        xyzw[i] /= xyzw[3];
- 
 
 221 
 
- 
 
 222 
    // Merge and mask
- 
 
 223 
    cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
- 
 
 224 
 
- 
 
 225 
}
- 
 
 226 
 
- 
 
 227 
void SMTriangulator::triangulateFromVp(cv::Mat &vp, cv::Mat &xyz){
- 
 
 228 
 
- 
 
 229 
    // Solve for xyzw using determinant tensor
- 
 
 230 
    cv::Mat C = determinantTensor;
- 
 
 231 
    std::vector<cv::Mat> xyzw(4);
- 
 
 232 
    for(unsigned int i=0; i<4; i++){
- 
 
 233 
        xyzw[i].create(vp.size(), CV_32F);
- 
 
 234 
        xyzw[i] = C.at<float>(cv::Vec4i(i,0,1,1)) - C.at<float>(cv::Vec4i(i,2,1,1))*uc - C.at<float>(cv::Vec4i(i,0,2,1))*vc -
- 
 
 235 
                C.at<float>(cv::Vec4i(i,0,1,2))*vp + C.at<float>(cv::Vec4i(i,2,1,2))*vp.mul(uc) + C.at<float>(cv::Vec4i(i,0,2,2))*vp.mul(vc);
- 
 
 236 
    }
- 
 
 237 
 
- 
 
 238 
    // Convert to non homogenous coordinates
- 
 
 239 
    for(unsigned int i=0; i<3; i++)
- 
 
 240 
        xyzw[i] /= xyzw[3];
- 
 
 241 
 
- 
 
 242 
    // Merge and mask
- 
 
 243 
    cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
- 
 
 244 
 
- 
 
 245 
}
- 
 
 246 
 
- 
 
 247 
void SMTriangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
- 
 
 248 
 
- 
 
 249 
    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
- 
 
 250 
    int N = up.rows * up.cols;
- 
 
 251 
 
- 
 
 252 
    cv::Mat projPointsCam(2, N, CV_32F);
- 
 
 253 
    uc.reshape(0,1).copyTo(projPointsCam.row(0));
- 
 
 254 
    vc.reshape(0,1).copyTo(projPointsCam.row(1));
- 
 
 255 
 
- 
 
 256 
    cv::Mat projPointsProj(2, N, CV_32F);
- 
 
 257 
    up.reshape(0,1).copyTo(projPointsProj.row(0));
- 
 
 258 
    vp.reshape(0,1).copyTo(projPointsProj.row(1));
- 
 
 259 
 
- 
 
 260 
    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
- 
 
 261 
    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
- 
 
 262 
 
- 
 
 263 
    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
- 
 
 264 
    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
- 
 
 265 
    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
- 
 
 266 
    Pp = cv::Mat(calibration.Kp) * temp;
- 
 
 267 
 
- 
 
 268 
    cv::Mat xyzw;
- 
 
 269 
    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
- 
 
 270 
 
- 
 
 271 
    xyz.create(3, N, CV_32F);
- 
 
 272 
    for(int i=0; i<N; i++){
- 
 
 273 
        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
- 
 
 274 
        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
- 
 
 275 
        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
- 
 
 276 
    }
- 
 
 277 
 
- 
 
 278 
    xyz = xyz.t();
- 
 
 279 
    xyz = xyz.reshape(3, up.rows);
- 
 
 280 
}
- 
 
 281 
 
118 
SMTriangulationWorker::~SMTriangulationWorker(){
 282 
SMSMTriangulator::~SMSMTriangulator(){
119 
    delete calibration;
 283 
    delete calibration;
120 
    delete triangulator;
 284 
    delete SMTriangulator;
121 
 
 285 
 
122 
    std::cout<<"triangulatorWorker deleted\n"<<std::flush;
 286 
    std::cout<<"SMTriangulatorWorker deleted\n"<<std::flush;
123 
}
 287 
}
- 
 
 288