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


#include "SMPointCloudWidget.h"
 
#include <opencv2/opencv.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(50, 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>();
 
    //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").value<SMCalibrationParameters>();
 
    // camera 0
    //...
 
    // rotation axis at (0,0,0) pointing (0,1,0) in rotation stage frame
    cv::Point3f O(0.0, 0.0, 0.0);
    cv::Vec3f v(0.0, 1.0, 0.0);
 
    // determine coefficients in camera 0 frame
    O = calibration.Rr*O + calibration.Tr;
    v = calibration.Rr*v + calibration.Tr;
 
    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->addLine(lineCoefficients);
}
 
SMPointCloudWidget::~SMPointCloudWidget(){
 
    //delete visualizer;
 
}
 

Rev 2	Rev 31
1	`#include "SMPointCloudWidget.h"`	1	`#include "SMPointCloudWidget.h"`
2		2
-		3	`#include <opencv2/opencv.hpp>`
-		4
3	`#include <vtkWindowToImageFilter.h>`	5	`#include <vtkWindowToImageFilter.h>`
4	`#include <vtkPNGWriter.h>`	6	`#include <vtkPNGWriter.h>`
5	`#include <vtkRenderWindow.h>`	7	`#include <vtkRenderWindow.h>`
6		8
7	`#include <pcl/point_cloud.h>`	9	`#include <pcl/point_cloud.h>`
8	`#include <pcl/point_types.h>`	10	`#include <pcl/point_types.h>`
9	`#include <pcl/common/io.h>`	11	`#include <pcl/common/io.h>`
10	`#include <pcl/visualization/point_cloud_color_handlers.h>`	12	`#include <pcl/visualization/point_cloud_color_handlers.h>`
11	`#include <pcl/geometry/quad_mesh.h>`	13	`#include <pcl/geometry/quad_mesh.h>`
12		14
13	`#include <pcl/io/pcd_io.h>`	15	`#include <pcl/io/pcd_io.h>`
14	`#include <pcl/io/ascii_io.h>`	16	`#include <pcl/io/ascii_io.h>`
15	`#include <pcl/io/ply_io.h>`	17	`#include <pcl/io/ply_io.h>`
16	`#include <pcl/io/png_io.h>`	18	`#include <pcl/io/png_io.h>`
17	`#include <pcl/io/vtk_io.h>`	19	`#include <pcl/io/vtk_io.h>`
18	`#include <vtkPolyDataWriter.h>`	20	`#include <vtkPolyDataWriter.h>`
19	`#include <pcl/conversions.h>`	21	`#include <pcl/conversions.h>`
20		22
21	`#include <fstream>`	23	`#include <fstream>`
22		24
23	`#include <QFileDialog>`	25	`#include <QFileDialog>`
-		26	`#include <QSettings>`
24		27
-		28	`#include "SMCalibrationParameters.h"`
25		29
26	`SMPointCloudWidget::SMPointCloudWidget(QWidget *parent) : QVTKWidget(parent) {`	30	`SMPointCloudWidget::SMPointCloudWidget(QWidget *parent) : QVTKWidget(parent) {`
27		31
28	`visualizer = new pcl::visualization::PCLVisualizer("PCLVisualizer", false);`	32	`visualizer = new pcl::visualization::PCLVisualizer("PCLVisualizer", false);`
29	`this->SetRenderWindow(visualizer->getRenderWindow());`	33	`this->SetRenderWindow(visualizer->getRenderWindow());`
30		34
31	`visualizer->setShowFPS(false);`	35	`visualizer->setShowFPS(false);`
32		36
33	`// Create point cloud viewport`	37	`// Create point cloud viewport`
34	`visualizer->setBackgroundColor(0, 0, 0);`	38	`visualizer->setBackgroundColor(0, 0, 0);`
35	`visualizer->addCoordinateSystem(50);`	39	`visualizer->addCoordinateSystem(50, 0);`
36	`#ifndef __APPLE__`	40	`#ifndef __APPLE__`
37	`// this leads to a segfault on OS X`	41	`// this leads to a segfault on OS X`
38	`visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);`	42	`visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);`
39	`#endif`	43	`#endif`
40		44
41	`// Initialize point cloud color handler`	45	`// Initialize point cloud color handler`
42	`colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>();`	46	`colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>();`
43		47
44	`//time.start();`	48	`//time.start();`
45	`}`	49	`}`
46		50
47	`void SMPointCloudWidget::updatePointCloud(PointCloudConstPtr _pointCloudPCL){`	51	`void SMPointCloudWidget::updatePointCloud(PointCloudConstPtr _pointCloudPCL){`
48		52
49	`// time.restart();`	53	`// time.restart();`
50		54
51	`pointCloudPCL = _pointCloudPCL;`	55	`pointCloudPCL = _pointCloudPCL;`
52		56
53	`// Note: using the color handler makes a copy of the rgb fields`	57	`// Note: using the color handler makes a copy of the rgb fields`
54	`colorHandler->setInputCloud(_pointCloudPCL);`	58	`colorHandler->setInputCloud(_pointCloudPCL);`
55		59
56	`if(!visualizer->updatePointCloud(pointCloudPCL, *colorHandler, "pointCloudPCL")){`	60	`if(!visualizer->updatePointCloud(pointCloudPCL, *colorHandler, "pointCloudPCL")){`
57	`visualizer->addPointCloud(pointCloudPCL, *colorHandler, "pointCloudPCL");`	61	`visualizer->addPointCloud(pointCloudPCL, *colorHandler, "pointCloudPCL");`
58	`visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, "pointCloudPCL");`	62	`visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, "pointCloudPCL");`
59	`}`	63	`}`
60		64
61	`this->update();`	65	`this->update();`
62	`emit newPointCloudDisplayed();`	66	`emit newPointCloudDisplayed();`
63		67
64	`// std::cout << "PCL Widget: " << time.restart() << "ms" << std::endl;`	68	`// std::cout << "PCL Widget: " << time.restart() << "ms" << std::endl;`
65	`}`	69	`}`
66		70
67	`void SMPointCloudWidget::savePointCloud(){`	71	`void SMPointCloudWidget::savePointCloud(){`
68		72
69	`QString selectedFilter;`	73	`QString selectedFilter;`
70	`QString fileName = QFileDialog::getSaveFileName(this, "Save Point Cloud", QString(), ".pcd;;.ply;;.vtk;;.png;;*.txt", &selectedFilter);`	74	`QString fileName = QFileDialog::getSaveFileName(this, "Save Point Cloud", QString(), ".pcd;;.ply;;.vtk;;.png;;*.txt", &selectedFilter);`
71	`QFileInfo info(fileName);`	75	`QFileInfo info(fileName);`
72	`QString type = info.suffix();`	76	`QString type = info.suffix();`
73	`if(type == ""){`	77	`if(type == ""){`
74	`fileName.append(selectedFilter.remove(0,1));`	78	`fileName.append(selectedFilter.remove(0,1));`
75	`type = selectedFilter.remove(0,1);`	79	`type = selectedFilter.remove(0,1);`
76	`}`	80	`}`
77		81
78	`if(type == "pcd"){`	82	`if(type == "pcd"){`
79	`pcl::io::savePCDFileASCII(fileName.toStdString(), *pointCloudPCL);`	83	`pcl::io::savePCDFileASCII(fileName.toStdString(), *pointCloudPCL);`
80	`} else if(type == "ply"){`	84	`} else if(type == "ply"){`
81	`//pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);`	85	`//pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);`
82	`pcl::PLYWriter w;`	86	`pcl::PLYWriter w;`
83	`// Write to ply in binary without camera`	87	`// Write to ply in binary without camera`
84	`w.write<pcl::PointXYZRGB> (fileName.toStdString(), *pointCloudPCL, true, false);`	88	`w.write<pcl::PointXYZRGB> (fileName.toStdString(), *pointCloudPCL, true, false);`
85	`} else if(type == "vtk"){`	89	`} else if(type == "vtk"){`
86	`pcl::PCLPointCloud2 pointCloud2;`	90	`pcl::PCLPointCloud2 pointCloud2;`
87	`pcl::toPCLPointCloud2(*pointCloudPCL, pointCloud2);`	91	`pcl::toPCLPointCloud2(*pointCloudPCL, pointCloud2);`
88	`pcl::io::saveVTKFile(fileName.toStdString(), pointCloud2);`	92	`pcl::io::saveVTKFile(fileName.toStdString(), pointCloud2);`
89		93
90	`// vtkPolyData polyData;`	94	`// vtkPolyData polyData;`
91	`// pcl::io::pointCloudTovtkPolyData(*pointCloudPCL, polyData);`	95	`// pcl::io::pointCloudTovtkPolyData(*pointCloudPCL, polyData);`
92	`// vtkPolyDataWriter::Pointer writer = vtkPolyDataWriter::New();`	96	`// vtkPolyDataWriter::Pointer writer = vtkPolyDataWriter::New();`
93	`// writer->SetInput(polyData);`	97	`// writer->SetInput(polyData);`
94	`// writer->SetFileName(fileName.toStdString());`	98	`// writer->SetFileName(fileName.toStdString());`
95	`// writer->Update();`	99	`// writer->Update();`
96	`} else if(type == "png"){`	100	`} else if(type == "png"){`
97	`pcl::io::savePNGFile(fileName.toStdString(), *pointCloudPCL, "rgb");`	101	`pcl::io::savePNGFile(fileName.toStdString(), *pointCloudPCL, "rgb");`
98	`} else if(type == "txt"){`	102	`} else if(type == "txt"){`
99	`std::ofstream s(fileName.toLocal8Bit());`	103	`std::ofstream s(fileName.toLocal8Bit());`
100	`for(unsigned int r=0; r<pointCloudPCL->height; r++){`	104	`for(unsigned int r=0; r<pointCloudPCL->height; r++){`
101	`for(unsigned int c=0; c<pointCloudPCL->width; c++){`	105	`for(unsigned int c=0; c<pointCloudPCL->width; c++){`
102	`pcl::PointXYZRGB p = pointCloudPCL->at(c, r);`	106	`pcl::PointXYZRGB p = pointCloudPCL->at(c, r);`
103	`if(p.x == p.x)`	107	`if(p.x == p.x)`
104	`s << p.x << " " << p.y << " " << p.z << "\r\n";`	108	`s << p.x << " " << p.y << " " << p.z << "\r\n";`
105	`}`	109	`}`
106	`}`	110	`}`
107	`std::flush(s);`	111	`std::flush(s);`
108	`s.close();`	112	`s.close();`
109	`}`	113	`}`
110		114
111	`}`	115	`}`
112		116
113	`void SMPointCloudWidget::saveScreenShot(){`	117	`void SMPointCloudWidget::saveScreenShot(){`
114		118
115	`vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();`	119	`vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();`
116	`//filter->SetInput(visualizer->getRenderWindow());`	120	`//filter->SetInput(visualizer->getRenderWindow());`
117	`filter->Modified();`	121	`filter->Modified();`
118		122
119	`QString fileName = QFileDialog::getSaveFileName(this, "Save Screen Shot", QString(), "*.png");`	123	`QString fileName = QFileDialog::getSaveFileName(this, "Save Screen Shot", QString(), "*.png");`
120	`QFileInfo info(fileName);`	124	`QFileInfo info(fileName);`
121	`QString type = info.suffix();`	125	`QString type = info.suffix();`
122	`if(type == "")`	126	`if(type == "")`
123	`fileName.append(".png");`	127	`fileName.append(".png");`
124		128
125	`vtkPNGWriter* writer = vtkPNGWriter::New();`	129	`vtkPNGWriter* writer = vtkPNGWriter::New();`
126	`writer->SetInput(filter->GetOutput());`	130	`writer->SetInput(filter->GetOutput());`
127	`writer->SetFileName(qPrintable(fileName));`	131	`writer->SetFileName(qPrintable(fileName));`
128	`writer->Write();`	132	`writer->Write();`
129	`writer->Delete();`	133	`writer->Delete();`
130	`}`	134	`}`
-		135
-		136	`void SMPointCloudWidget::updateCalibrationParameters(){`
-		137
-		138	`QSettings settings;`
-		139	`SMCalibrationParameters calibration = settings.value("calibration").value<SMCalibrationParameters>();`
-		140
-		141	`// camera 0`
-		142	`//...`
-		143
-		144	`// rotation axis at (0,0,0) pointing (0,1,0) in rotation stage frame`
-		145	`cv::Point3f O(0.0, 0.0, 0.0);`
-		146	`cv::Vec3f v(0.0, 1.0, 0.0);`
-		147
-		148	`// determine coefficients in camera 0 frame`
-		149	`O = calibration.Rr*O + calibration.Tr;`
-		150	`v = calibration.Rr*v + calibration.Tr;`
-		151
-		152	`pcl::ModelCoefficients lineCoefficients;`
-		153	`lineCoefficients.values.resize(6);`
-		154	`lineCoefficients.values[0] = O[0];`
-		155	`lineCoefficients.values[1] = O[1];`
-		156	`lineCoefficients.values[2] = O[2];`
-		157	`lineCoefficients.values[3] = v[0];`
-		158	`lineCoefficients.values[4] = v[1];`
-		159	`lineCoefficients.values[5] = v[2];`
-		160	`visualizer->addLine(lineCoefficients);`
-		161	`}`
131		162
132	`SMPointCloudWidget::~SMPointCloudWidget(){`	163	`SMPointCloudWidget::~SMPointCloudWidget(){`
133		164
134	`//delete visualizer;`	165	`//delete visualizer;`
135		166
136	`}`	167	`}`
137		168

Subversion Repositories seema-scanner

(root)/src/SMPointCloudWidget.cpp – Rev 2 → 31