WebSVN – seema-scanner – Diff – /src/SMTriangulator.cpp

-#include "SMSMTriangulator.h"
+#include "SMTriangulator.h"
 #include <QCoreApplication>
 #include <QSettings>
 #include <iostream>
 #include <opencv2/opencv.hpp>
 #include <pcl/filters/statistical_outlier_removal.h>
 #include <pcl/io/pcd_io.h>
-void SMSMTriangulator::setup(){
-    // Initialize SMTriangulator with calibration
+SMTriangulator::SMTriangulator(){
-    calibration = new SMCalibrationParams;
-    calibration->load("calibration.yml");
-    SMTriangulator = new SMTriangulator(*calibration, frameWidth, frameHeight);
-    QSettings settings("SLStudio");
+    // Precompute lens correction maps
-    writeToDisk = settings.value("writeToDisk/pointclouds",false).toBool();
+    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
+    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1, lensMap2);
+    //cv::Mat map1, map2;
+    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
+    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
+    //cv::imwrite("map1.png", map1);
+    //cv::imwrite("map2.png", map2);
+}
-void SMSMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
+void SMTriangulator::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
     time.start();
     // Reconstruct point cloud
     cv::Mat pointCloud;
     cv::Mat empty;
-    SMTriangulator->triangulate(up, vp, mask, shading, pointCloud);
+    triangulate(up, vp, mask, shading, pointCloud);
     std::vector<cv::Mat> xyz;
     cv::split(pointCloud, xyz);
 //    emit imshow("x", xyz[0], 1400, 100);
 //    emit imshow("y", xyz[1], 1400, 450);
 //    emit imshow("z", xyz[2], 1400, 800);
     // Convert point cloud to PCL format
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
     // Interprete as organized point cloud
     pointCloudPCL->width = pointCloud.cols;
     pointCloudPCL->height = pointCloud.rows;
     pointCloudPCL->is_dense = false;
     pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);
     //    for(int col=0; col<pointCloud.cols; col++){
     //        for(int row=0; row<pointCloud.rows; row++){
     //            const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
     //            unsigned char shade = shading.at<unsigned char>(row,col);
     //            pcl::PointXYZRGBRGB point;
     //            point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
     //            point.r = shade; point.g = shade; point.b = shade;
     //            pointCloudPCL->at(col, row) = point;
     //        }
     //    }
     // stack xyz data
     std::vector<cv::Mat> pointCloudChannels;
     pointCloudChannels.push_back(xyz[0]);
     pointCloudChannels.push_back(xyz[1]);
     pointCloudChannels.push_back(xyz[2]);
     // 4 byte padding
     pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
     // triple uchar color information
     std::vector<cv::Mat> rgb;
     rgb.push_back(shading);
     rgb.push_back(shading);
     rgb.push_back(shading);
     rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));
     cv::Mat rgb8UC4;
     cv::merge(rgb, rgb8UC4);
     cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);
     pointCloudChannels.push_back(rgb32F);
     // 12 bytes padding
     pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
     pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
     pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
     // merge channels
     cv::Mat pointCloudPadded;
     cv::merge(pointCloudChannels, pointCloudPadded);
     // memcpy everything
     memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));
 //    // filtering
 //    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> filter;
 //    filter.setMeanK(5);
 //    filter.setStddevMulThresh(1.0);
 //    filter.setInputCloud(pointCloudPCL);
 //    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
 //    filter.filter(*pointCloudFiltered);
     // Emit result
     emit newPointCloud(pointCloudPCL);
     std::cout << "SMTriangulator: " << time.elapsed() << "ms" << std::endl;
-    if(writeToDisk){
-        QString fileName = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmsszzz");
-        fileName.append(".pcd");
-        pcl::io::savePCDFileBinary(fileName.toStdString(), *pointCloudPCL);
-    }
     //emit finished();
+}
-SMTriangulator::SMTriangulator(SMCalibrationParams _calibration, unsigned int frameWidth, unsigned int frameHeight) : calibration(_calibration){
-    // Precompute uc, vc maps
-    uc.create(frameHeight, frameWidth, CV_32F);
-    vc.create(frameHeight, frameWidth, CV_32F);
-    for(unsigned int row=0; row<frameHeight; row++){
-        for(unsigned int col=0; col<frameWidth; col++){
-            uc.at<float>(row, col) = col;
-            vc.at<float>(row, col) = row;
-        }
-    }
-    // Precompute determinant tensor
-    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
-    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
-    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
-    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
-    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
-    Pp = cv::Mat(calibration.Kp) * temp;
-    cv::Mat e = cv::Mat::eye(4, 4, CV_32F);
-    int sz[] = {4, 3, 3, 3};
-    cv::Mat C(4, sz, CV_32F, cv::Scalar::all(0));
-    for(int k=0; k<4; k++){
-        for(int i=0; i<3; i++){
-            for(int j=0; j<3; j++){
-                for(int l=0; l<3; l++){
-                    cv::Mat op(4, 4, CV_32F);
-                    Pc.row(i).copyTo(op.row(0));
-                    Pc.row(j).copyTo(op.row(1));
-                    Pp.row(l).copyTo(op.row(2));
-                    e.row(k).copyTo(op.row(3));
-                    C.at<float>(cv::Vec4i(k,i,j,l)) = cv::determinant(op.t());
-                }
-            }
-        }
-    }
-    determinantTensor = C;
-    // Precompute lens correction maps
-    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
-    cv::initUndistortRectifyMap(calibration.Kc, calibration.kc, eye, calibration.Kc, cv::Size(frameWidth, frameHeight), CV_32FC1, lensMap1, lensMap2);
-    //cv::Mat map1, map2;
-    //cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
-    //cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
-    //cv::imwrite("map1.png", map1);
-    //cv::imwrite("map2.png", map2);
-}
 void SMTriangulator::triangulate(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading, cv::Mat &pointCloud){
     // Undistort up, mask and shading
     if(!up.empty()){
         cv::Mat upUndistort;
         cv::remap(up, upUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
         up = upUndistort;
+    }
     if(!vp.empty()){
         cv::Mat vpUndistort;
         cv::remap(vp, vpUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
         vp = vpUndistort;
+    }
     cv::Mat maskUndistort, shadingUndistort;
     cv::remap(mask, maskUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
     cv::remap(shading, shadingUndistort, lensMap1, lensMap2, cv::INTER_LINEAR);
     mask = maskUndistort;
     shading = shadingUndistort;
     // Triangulate
     cv::Mat xyz;
     if(!up.empty() && vp.empty())
         triangulateFromUp(up, xyz);
     else if(!vp.empty() && up.empty())
         triangulateFromVp(vp, xyz);
     else if(!up.empty() && !vp.empty())
         triangulateFromUpVp(up, vp, xyz);
     // Merge and mask
     pointCloud = cv::Mat(up.size(), CV_32FC3, cv::Scalar(NAN, NAN, NAN));
     xyz.copyTo(pointCloud, mask);
+}
 void SMTriangulator::triangulateFromUp(cv::Mat &up, cv::Mat &xyz){
     // Solve for xyzw using determinant tensor
     cv::Mat C = determinantTensor;
     std::vector<cv::Mat> xyzw(4);
     for(unsigned int i=0; i<4; i++){
         xyzw[i].create(up.size(), CV_32F);
         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 -
                 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);
+    }
     // Convert to non homogenous coordinates
     for(unsigned int i=0; i<3; i++)
         xyzw[i] /= xyzw[3];
     // Merge and mask
     cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
+}
 void SMTriangulator::triangulateFromVp(cv::Mat &vp, cv::Mat &xyz){
     // Solve for xyzw using determinant tensor
     cv::Mat C = determinantTensor;
     std::vector<cv::Mat> xyzw(4);
     for(unsigned int i=0; i<4; i++){
         xyzw[i].create(vp.size(), CV_32F);
         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 -
                 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);
+    }
     // Convert to non homogenous coordinates
     for(unsigned int i=0; i<3; i++)
         xyzw[i] /= xyzw[3];
     // Merge and mask
     cv::merge(std::vector<cv::Mat>(xyzw.begin(), xyzw.begin()+3), xyz);
+}
 void SMTriangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
-    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
+//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
-    int N = up.rows * up.cols;
+//    int N = up.rows * up.cols;
-    cv::Mat projPointsCam(2, N, CV_32F);
+//    cv::Mat projPointsCam(2, N, CV_32F);
-    uc.reshape(0,1).copyTo(projPointsCam.row(0));
+//    uc.reshape(0,1).copyTo(projPointsCam.row(0));
-    vc.reshape(0,1).copyTo(projPointsCam.row(1));
+//    vc.reshape(0,1).copyTo(projPointsCam.row(1));
-    cv::Mat projPointsProj(2, N, CV_32F);
+//    cv::Mat projPointsProj(2, N, CV_32F);
-    up.reshape(0,1).copyTo(projPointsProj.row(0));
+//    up.reshape(0,1).copyTo(projPointsProj.row(0));
-    vp.reshape(0,1).copyTo(projPointsProj.row(1));
+//    vp.reshape(0,1).copyTo(projPointsProj.row(1));
-    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
+//    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
-    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
+//    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
-    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
+//    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
-    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
+//    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
-    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
+//    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
-    Pp = cv::Mat(calibration.Kp) * temp;
+//    Pp = cv::Mat(calibration.Kp) * temp;
-    cv::Mat xyzw;
+//    cv::Mat xyzw;
-    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
+//    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
-    xyz.create(3, N, CV_32F);
+//    xyz.create(3, N, CV_32F);
-    for(int i=0; i<N; i++){
+//    for(int i=0; i<N; i++){
-        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
+//        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
-        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
+//        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
-        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
+//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
-    }
+//    }
-    xyz = xyz.t();
+//    xyz = xyz.t();
-    xyz = xyz.reshape(3, up.rows);
+//    xyz = xyz.reshape(3, up.rows);
+}
-SMSMTriangulator::~SMSMTriangulator(){
+SMTriangulator::~SMTriangulator(){
-    delete calibration;
-    delete SMTriangulator;
-    std::cout<<"SMTriangulatorWorker deleted\n"<<std::flush;
+    std::cout << "SMTriangulator deleted.\n" << std::flush;
+}

Subversion Repositories seema-scanner

(root)/src/SMTriangulator.cpp @ 25 – Rev 24 → 25