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 <pcl/io/pcd_io.h>

#include <pcl/io/ascii_io.h>

#include <pcl/io/ply_io.h>

#include <pcl/io/png_io.h>

#include <pcl/io/vtk_io.h>

#include <vtkPolyDataWriter.h>

#include <pcl/conversions.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, 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::PointXYZRGB>();

    updateCalibrationParameters();

    //time.start();

}

void SMPointCloudWidget::updatePointCloud(PointCloudConstPtr _pointCloudPCL){

//    time.restart();

    pointCloudPCL = _pointCloudPCL;

    // Note: using the color handler makes a copy of the rgb fields

    colorHandler->setInputCloud(_pointCloudPCL);

    if(!visualizer->updatePointCloud(pointCloudPCL, *colorHandler, "pointCloudPCL")){

        visualizer->addPointCloud(pointCloudPCL, *colorHandler, "pointCloudPCL");

        visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, "pointCloudPCL");

     }

    this->update();

    emit newPointCloudDisplayed();

//    std::cout << "PCL Widget: " << time.restart() << "ms" << std::endl;

}

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::PointXYZRGB> (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::PointXYZRGB 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(0);

    visualizer->addCoordinateSystem(100, 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());

    // 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 at (0,0,0) pointing (0,1,0) in rotation stage frame

    cv::Vec3f O(0.0, -100.0, 0.0);

    cv::Vec3f v(0.0, 300.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;

}