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


#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>();
 
    // camear 0 coordinate system
    visualizer->removeCoordinateSystem();
    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 at (0,0,0) pointing (0,1,0) in rotation stage frame
    cv::Vec3f 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->removeShape("line");
    visualizer->addLine(lineCoefficients);
}
 
SMPointCloudWidget::~SMPointCloudWidget(){
 
    //delete visualizer;
 
}
 

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

Subversion Repositories seema-scanner

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