Subversion Repositories seema-scanner

#include "SMPointCloudWidget.h"

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include <vtkWindowToImageFilter.h>
#include <vtkPNGWriter.h>
#include <vtkRenderWindow.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <pcl/geometry/quad_mesh.h>

#include <fstream>


#include <QFileDialog>
#include <QSettings>

#include "SMCalibrationParameters.h"

SMPointCloudWidget::SMPointCloudWidget(QWidget *parent) : QVTKWidget(parent) {

    visualizer = new pcl::visualization::PCLVisualizer("PCLVisualizer", false);
    this->SetRenderWindow(visualizer->getRenderWindow());

    visualizer->setShowFPS(false);

    // Create point cloud viewport
    visualizer->setBackgroundColor(0, 0, 0);
    visualizer->addCoordinateSystem(100, "camera0", 0);
#ifndef __APPLE__
    // this leads to a segfault on OS X
    visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);
#endif

    // Initialize point cloud color handler
    colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>();

    updateCalibrationParameters();
    //time.start();
}

void SMPointCloudWidget::addPointCloud(SMPointCloud pointCloud, int id){

    std::string stringId = QString::number(id).toStdString();
//    std::string stringId = QString("id%1").arg(id).toStdString();

    // Note: using the color handler makes a copy of the rgb fields
    colorHandler->setInputCloud(pointCloud.pointCloud);

    if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, stringId)){
        visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);
        visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);
     }

    // transformation matrix in Eigen format
    cv::Mat TransformCV(3, 4, CV_32F);
    cv::Mat(pointCloud.R).copyTo(TransformCV.colRange(0, 3));
    cv::Mat(pointCloud.T).copyTo(TransformCV.col(3));
    Eigen::Affine3f Transform;
    cv::cv2eigen(TransformCV, Transform.matrix());

    visualizer->updatePointCloudPose(stringId, Transform.inverse());

    this->update();

    emit pointCloudDataChanged();

}

void SMPointCloudWidget::updatePointCloud(SMPointCloud pointCloud, int id){

    std::string stringId = QString::number(id).toStdString();
//    std::string stringId = QString("id%1").arg(id).toStdString();

    // Note: using the color handler makes a copy of the rgb fields
    colorHandler->setInputCloud(pointCloud.pointCloud);

    if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, stringId)){
        visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);
        visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);
     }

    this->update();
    emit pointCloudDataChanged();

}

void SMPointCloudWidget::removePointCloud(int id){

    std::string stringId = QString::number(id).toStdString();
//    std::string stringId = QString("id%1").arg(id).toStdString();

    visualizer->removePointCloud(stringId);

    this->update();
    emit pointCloudDataChanged();

}

//void SMPointCloudWidget::savePointCloud(){

//    QString selectedFilter;
//    QString fileName = QFileDialog::getSaveFileName(this, "Save Point Cloud", QString(), "*.pcd;;*.ply;;*.vtk;;*.png;;*.txt", &selectedFilter);
//    QFileInfo info(fileName);
//    QString type = info.suffix();
//    if(type == ""){
//        fileName.append(selectedFilter.remove(0,1));
//        type = selectedFilter.remove(0,1);
//    }

//    if(type == "pcd"){
//        pcl::io::savePCDFileASCII(fileName.toStdString(), *pointCloudPCL);
//    } else if(type == "ply"){
//        //pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);
//        pcl::PLYWriter w;
//        // Write to ply in binary without camera
//        w.write<pcl::PointXYZRGBNormal> (fileName.toStdString(), *pointCloudPCL, true, false);
//    } else if(type == "vtk"){
//        pcl::PCLPointCloud2 pointCloud2;
//        pcl::toPCLPointCloud2(*pointCloudPCL, pointCloud2);
//        pcl::io::saveVTKFile(fileName.toStdString(), pointCloud2);

////        vtkPolyData polyData;
////        pcl::io::pointCloudTovtkPolyData(*pointCloudPCL, polyData);
////        vtkPolyDataWriter::Pointer writer = vtkPolyDataWriter::New();
////        writer->SetInput(polyData);
////        writer->SetFileName(fileName.toStdString());
////        writer->Update();
//    } else if(type == "png"){
//        pcl::io::savePNGFile(fileName.toStdString(), *pointCloudPCL, "rgb");
//    } else if(type == "txt"){
//        std::ofstream s(fileName.toLocal8Bit());
//        for(unsigned int r=0; r<pointCloudPCL->height; r++){
//            for(unsigned int c=0; c<pointCloudPCL->width; c++){
//                pcl::PointXYZRGBNormal p = pointCloudPCL->at(c, r);
//                if(p.x == p.x)
//                    s << p.x << " " << p.y << " " << p.z << "\r\n";
//            }
//        }
//        std::flush(s);
//        s.close();
//    }

//}

void SMPointCloudWidget::saveScreenShot(){

    vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();
    //filter->SetInput(visualizer->getRenderWindow());
    filter->Modified();

    QString fileName = QFileDialog::getSaveFileName(this, "Save Screen Shot", QString(), "*.png");
    QFileInfo info(fileName);
    QString type = info.suffix();
    if(type == "")
        fileName.append(".png");

    vtkPNGWriter* writer = vtkPNGWriter::New();
    writer->SetInput(filter->GetOutput());
    writer->SetFileName(qPrintable(fileName));
    writer->Write();
    writer->Delete();
}

void SMPointCloudWidget::updateCalibrationParameters(){

    QSettings settings;
    SMCalibrationParameters calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();

    // camera 0 coordinate system
    visualizer->removeCoordinateSystem("camera0", 0);
    visualizer->addCoordinateSystem(100, "camera0", 0);

    // camera 1 coordinate system
    cv::Mat Transform1CV(3, 4, CV_32F);
    cv::Mat(calibration.R1).copyTo(Transform1CV.colRange(0, 3));
    cv::Mat(calibration.T1).copyTo(Transform1CV.col(3));
    Eigen::Affine3f Transform1;
    cv::cv2eigen(Transform1CV, Transform1.matrix());

    visualizer->addCoordinateSystem(100, Transform1.inverse(), "camera1", 0);

    // rotation axis coordinate system
//    cv::Mat TransformRCV(3, 4, CV_32F);
//    cv::Mat(calibration.Rr).copyTo(TransformRCV.colRange(0, 3));
//    cv::Mat(calibration.Tr).copyTo(TransformRCV.col(3));
//    Eigen::Affine3f TransformR;
//    cv::cv2eigen(TransformRCV, TransformR.matrix());

//    visualizer->addCoordinateSystem(100, TransformR);

    // rotation axis pointing (0,1,0) in rotation stage frame
    cv::Vec3f O(0.0, 00.0, 0.0);
    cv::Vec3f v(0.0, 600.0, 0.0);

    // determine coefficients in camera 0 frame
    O = calibration.Rr.t()*O + -calibration.Rr.t()*calibration.Tr;
    v = calibration.Rr.t()*v + -calibration.Rr.t()*calibration.Tr - O; 

    pcl::ModelCoefficients lineCoefficients;
    lineCoefficients.values.resize(6);
    lineCoefficients.values[0] = O[0];
    lineCoefficients.values[1] = O[1];
    lineCoefficients.values[2] = O[2];
    lineCoefficients.values[3] = v[0];
    lineCoefficients.values[4] = v[1];
    lineCoefficients.values[5] = v[2];
    visualizer->removeShape("line");
    visualizer->addLine(lineCoefficients);   

}

SMPointCloudWidget::~SMPointCloudWidget(){

    //delete visualizer;

}