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 <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->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
 
    // Disable double buffering (which causes lag in VTK6)
    visualizer->getRenderWindow()->SetDoubleBuffer(0);
 
    visualizer->setShowFPS(false);
 
    // Create point cloud viewport
    visualizer->setBackgroundColor(0, 0, 0);
    visualizer->addCoordinateSystem(100, "camera0", 0);
    visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);
 
 
    // 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->repaint();
 
    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->repaint();
    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->repaint();
    emit pointCloudDataChanged();
 
}
 
void SMPointCloudWidget::removeAllPointClouds(){
 
    visualizer->removeAllPointClouds();
 
    this->update();
    emit pointCloudDataChanged();
 
}
 
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->SetFileName(qPrintable(fileName));
    writer->SetInputConnection(filter->GetOutputPort());
    writer->Write();
    writer->Delete();
}
 
void SMPointCloudWidget::updateCalibrationParameters(){
 
    QSettings settings;
    QVariant calibrationVariant = settings.value("calibration/parameters");
    SMCalibrationParameters calibration = calibrationVariant.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->removeCoordinateSystem("camera1", 0);
    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->removeCoordinateSystem("camera1", 0);
//    visualizer->addCoordinateSystem(100, TransformR.inverse(), "rotation", 0);
 
    // rotation axis pointing (0,1,0) in rotation stage frame
    cv::Vec3f O(0.0, 0.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);
 
 
    this->repaint();
 
}
 
SMPointCloudWidget::~SMPointCloudWidget(){
 
    //delete visualizer;
 
}
 

Subversion Repositories seema-scanner

(root)/src/SMPointCloudWidget.cpp – Rev 208 → 236

Rev 208		Rev 236
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	`#include <opencv2/core/eigen.hpp>`
5		5
6	`#include <vtkWindowToImageFilter.h>`	6	`#include <vtkWindowToImageFilter.h>`
7	`#include <vtkPNGWriter.h>`	7	`#include <vtkPNGWriter.h>`
8	`#include <vtkRenderWindow.h>`	8	`#include <vtkRenderWindow.h>`
9		9
10	`#include <pcl/point_cloud.h>`	10	`#include <pcl/point_cloud.h>`
11	`#include <pcl/point_types.h>`	11	`#include <pcl/point_types.h>`
12	`#include <pcl/common/io.h>`	12	`#include <pcl/common/io.h>`
13	`#include <pcl/visualization/point_cloud_color_handlers.h>`	13	`#include <pcl/visualization/point_cloud_color_handlers.h>`
14	`#include <pcl/geometry/quad_mesh.h>`	14	`#include <pcl/geometry/quad_mesh.h>`
15		15
16	`#include <fstream>`	16	`#include <fstream>`
17		17
18		18
19	`#include <QFileDialog>`	19	`#include <QFileDialog>`
20	`#include <QSettings>`	20	`#include <QSettings>`
21		21
22	`#include "SMCalibrationParameters.h"`	22	`#include "SMCalibrationParameters.h"`
23		23
24	`SMPointCloudWidget::SMPointCloudWidget(QWidget *parent) : QVTKWidget(parent) {`	24	`SMPointCloudWidget::SMPointCloudWidget(QWidget *parent) : QVTKWidget(parent) {`
25		25
26	`visualizer = new pcl::visualization::PCLVisualizer("PCLVisualizer", false);`	26	`visualizer = new pcl::visualization::PCLVisualizer("PCLVisualizer", false);`
27	`this->SetRenderWindow(visualizer->getRenderWindow());`	27	`this->SetRenderWindow(visualizer->getRenderWindow());`
28		28
29	`visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());`	29	`visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());`
30		30
31	`// Disable double buffering (which causes lag in VTK6)`	31	`// Disable double buffering (which causes lag in VTK6)`
32	`visualizer->getRenderWindow()->SetDoubleBuffer(0);`	32	`visualizer->getRenderWindow()->SetDoubleBuffer(0);`
33		33
34	`visualizer->setShowFPS(false);`	34	`visualizer->setShowFPS(false);`
35		35
36	`// Create point cloud viewport`	36	`// Create point cloud viewport`
37	`visualizer->setBackgroundColor(0, 0, 0);`	37	`visualizer->setBackgroundColor(0, 0, 0);`
38	`visualizer->addCoordinateSystem(100, "camera0", 0);`	38	`visualizer->addCoordinateSystem(100, "camera0", 0);`
39	`visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);`	39	`visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);`
40		40
41		41
42	`// Initialize point cloud color handler`	42	`// Initialize point cloud color handler`
43	`colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>();`	43	`colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>();`
44		44
45	`updateCalibrationParameters();`	45	`updateCalibrationParameters();`
46	`//time.start();`	46	`//time.start();`
47	`}`	47	`}`
48		48
49	`void SMPointCloudWidget::addPointCloud(SMPointCloud pointCloud, int id){`	49	`void SMPointCloudWidget::addPointCloud(SMPointCloud pointCloud, int id){`
50		50
51	`std::string stringId = QString::number(id).toStdString();`	51	`std::string stringId = QString::number(id).toStdString();`
52	`// std::string stringId = QString("id%1").arg(id).toStdString();`	52	`// std::string stringId = QString("id%1").arg(id).toStdString();`
53		53
54	`// Note: using the color handler makes a copy of the rgb fields`	54	`// Note: using the color handler makes a copy of the rgb fields`
55	`colorHandler->setInputCloud(pointCloud.pointCloud);`	55	`colorHandler->setInputCloud(pointCloud.pointCloud);`
56		56
57	`if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, stringId)){`	57	`if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, stringId)){`
58	`visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);`	58	`visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);`
59	`visualizer->setPointCloudRenderingProperties(`	59	`visualizer->setPointCloudRenderingProperties(`
60	`pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);`	60	`pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);`
61	`}`	61	`}`
62		62
63	`// transformation matrix in Eigen format`	63	`// transformation matrix in Eigen format`
64	`cv::Mat TransformCV(3, 4, CV_32F);`	64	`cv::Mat TransformCV(3, 4, CV_32F);`
65	`cv::Mat(pointCloud.R).copyTo(TransformCV.colRange(0, 3));`	65	`cv::Mat(pointCloud.R).copyTo(TransformCV.colRange(0, 3));`
66	`cv::Mat(pointCloud.T).copyTo(TransformCV.col(3));`	66	`cv::Mat(pointCloud.T).copyTo(TransformCV.col(3));`
67	`Eigen::Affine3f Transform;`	67	`Eigen::Affine3f Transform;`
68	`cv::cv2eigen(TransformCV, Transform.matrix());`	68	`cv::cv2eigen(TransformCV, Transform.matrix());`
69		69
70	`visualizer->updatePointCloudPose(stringId, Transform.inverse());`	70	`visualizer->updatePointCloudPose(stringId, Transform.inverse());`
71		71
72	`this->update();`	72	`this->repaint();`
73		73
74	`emit pointCloudDataChanged();`	74	`emit pointCloudDataChanged();`
75		75
76	`}`	76	`}`
77		77
78	`void SMPointCloudWidget::updatePointCloud(SMPointCloud pointCloud, int id){`	78	`void SMPointCloudWidget::updatePointCloud(SMPointCloud pointCloud, int id){`
79		79
80	`std::string stringId = QString::number(id).toStdString();`	80	`std::string stringId = QString::number(id).toStdString();`
81	`// std::string stringId = QString("id%1").arg(id).toStdString();`	81	`// std::string stringId = QString("id%1").arg(id).toStdString();`
82		82
83	`// Note: using the color handler makes a copy of the rgb fields`	83	`// Note: using the color handler makes a copy of the rgb fields`
84	`colorHandler->setInputCloud(pointCloud.pointCloud);`	84	`colorHandler->setInputCloud(pointCloud.pointCloud);`
85		85
86	`if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, stringId)){`	86	`if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, stringId)){`
87	`visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);`	87	`visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);`
88	`visualizer->setPointCloudRenderingProperties(`	88	`visualizer->setPointCloudRenderingProperties(`
89	`pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);`	89	`pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);`
90	`}`	90	`}`
91		91
92	`this->update();`	92	`this->repaint();`
93	`emit pointCloudDataChanged();`	93	`emit pointCloudDataChanged();`
94		94
95	`}`	95	`}`
96		96
97	`void SMPointCloudWidget::removePointCloud(int id){`	97	`void SMPointCloudWidget::removePointCloud(int id){`
98		98
99	`std::string stringId = QString::number(id).toStdString();`	99	`std::string stringId = QString::number(id).toStdString();`
100	`// std::string stringId = QString("id%1").arg(id).toStdString();`	100	`// std::string stringId = QString("id%1").arg(id).toStdString();`
101		101
102	`visualizer->removePointCloud(stringId);`	102	`visualizer->removePointCloud(stringId);`
103		103
104	`this->update();`	104	`this->repaint();`
105	`emit pointCloudDataChanged();`	105	`emit pointCloudDataChanged();`
106		106
107	`}`	107	`}`
108		108
109	`void SMPointCloudWidget::removeAllPointClouds(){`	109	`void SMPointCloudWidget::removeAllPointClouds(){`
110		110
111	`visualizer->removeAllPointClouds();`	111	`visualizer->removeAllPointClouds();`
112		112
113	`this->update();`	113	`this->update();`
114	`emit pointCloudDataChanged();`	114	`emit pointCloudDataChanged();`
115		115
116	`}`	116	`}`
117		117
118	`void SMPointCloudWidget::saveScreenShot(){`	118	`void SMPointCloudWidget::saveScreenShot(){`
119		119
120	`vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();`	120	`vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();`
121	`//filter->SetInput(visualizer->getRenderWindow());`	121	`//filter->SetInput(visualizer->getRenderWindow());`
122	`filter->Modified();`	122	`filter->Modified();`
123		123
124	`QString fileName = QFileDialog::getSaveFileName(this, "Save Screen Shot", QString(), "*.png");`	124	`QString fileName = QFileDialog::getSaveFileName(this, "Save Screen Shot", QString(), "*.png");`
125	`QFileInfo info(fileName);`	125	`QFileInfo info(fileName);`
126	`QString type = info.suffix();`	126	`QString type = info.suffix();`
127	`if(type == "")`	127	`if(type == "")`
128	`fileName.append(".png");`	128	`fileName.append(".png");`
129		129
130	`vtkPNGWriter* writer = vtkPNGWriter::New();`	130	`vtkPNGWriter* writer = vtkPNGWriter::New();`
131	`writer->SetFileName(qPrintable(fileName));`	131	`writer->SetFileName(qPrintable(fileName));`
132	`writer->SetInputConnection(filter->GetOutputPort());`	132	`writer->SetInputConnection(filter->GetOutputPort());`
133	`writer->Write();`	133	`writer->Write();`
134	`writer->Delete();`	134	`writer->Delete();`
135	`}`	135	`}`
136		136
137	`void SMPointCloudWidget::updateCalibrationParameters(){`	137	`void SMPointCloudWidget::updateCalibrationParameters(){`
138		138
139	`QSettings settings;`	139	`QSettings settings;`
140	`QVariant calibrationVariant = settings.value("calibration/parameters");`	140	`QVariant calibrationVariant = settings.value("calibration/parameters");`
141	`SMCalibrationParameters calibration = calibrationVariant.value<SMCalibrationParameters>();`	141	`SMCalibrationParameters calibration = calibrationVariant.value<SMCalibrationParameters>();`
142		142
143	`// camera 0 coordinate system`	143	`// camera 0 coordinate system`
144	`visualizer->removeCoordinateSystem("camera0", 0);`	144	`visualizer->removeCoordinateSystem("camera0", 0);`
145	`visualizer->addCoordinateSystem(100, "camera0", 0);`	145	`visualizer->addCoordinateSystem(100, "camera0", 0);`
146		146
147	`// camera 1 coordinate system`	147	`// camera 1 coordinate system`
148	`cv::Mat Transform1CV(3, 4, CV_32F);`	148	`cv::Mat Transform1CV(3, 4, CV_32F);`
149	`cv::Mat(calibration.R1).copyTo(Transform1CV.colRange(0, 3));`	149	`cv::Mat(calibration.R1).copyTo(Transform1CV.colRange(0, 3));`
150	`cv::Mat(calibration.T1).copyTo(Transform1CV.col(3));`	150	`cv::Mat(calibration.T1).copyTo(Transform1CV.col(3));`
151	`Eigen::Affine3f Transform1;`	151	`Eigen::Affine3f Transform1;`
152	`cv::cv2eigen(Transform1CV, Transform1.matrix());`	152	`cv::cv2eigen(Transform1CV, Transform1.matrix());`
153		153
154	`visualizer->removeCoordinateSystem("camera1", 0);`	154	`visualizer->removeCoordinateSystem("camera1", 0);`
155	`visualizer->addCoordinateSystem(100, Transform1.inverse(), "camera1", 0);`	155	`visualizer->addCoordinateSystem(100, Transform1.inverse(), "camera1", 0);`
156		156
157	`// // rotation axis coordinate system`	157	`// // rotation axis coordinate system`
158	`// cv::Mat TransformRCV(3, 4, CV_32F);`	158	`// cv::Mat TransformRCV(3, 4, CV_32F);`
159	`// cv::Mat(calibration.Rr).copyTo(TransformRCV.colRange(0, 3));`	159	`// cv::Mat(calibration.Rr).copyTo(TransformRCV.colRange(0, 3));`
160	`// cv::Mat(calibration.Tr).copyTo(TransformRCV.col(3));`	160	`// cv::Mat(calibration.Tr).copyTo(TransformRCV.col(3));`
161	`// Eigen::Affine3f TransformR;`	161	`// Eigen::Affine3f TransformR;`
162		162
163	`// cv::cv2eigen(TransformRCV, TransformR.matrix());`	163	`// cv::cv2eigen(TransformRCV, TransformR.matrix());`
164	`// visualizer->removeCoordinateSystem("camera1", 0);`	164	`// visualizer->removeCoordinateSystem("camera1", 0);`
165	`// visualizer->addCoordinateSystem(100, TransformR.inverse(), "rotation", 0);`	165	`// visualizer->addCoordinateSystem(100, TransformR.inverse(), "rotation", 0);`
166		166
167	`// rotation axis pointing (0,1,0) in rotation stage frame`	167	`// rotation axis pointing (0,1,0) in rotation stage frame`
168	`cv::Vec3f O(0.0, 0.0, 0.0);`	168	`cv::Vec3f O(0.0, 0.0, 0.0);`
169	`cv::Vec3f v(0.0, 600.0, 0.0);`	169	`cv::Vec3f v(0.0, 600.0, 0.0);`
170		170
171	`// determine coefficients in camera 0 frame`	171	`// determine coefficients in camera 0 frame`
172	`O = calibration.Rr.t()O + -calibration.Rr.t()calibration.Tr;`	172	`O = calibration.Rr.t()O + -calibration.Rr.t()calibration.Tr;`
173	`v = calibration.Rr.t()v + -calibration.Rr.t()calibration.Tr - O;`	173	`v = calibration.Rr.t()v + -calibration.Rr.t()calibration.Tr - O;`
174		174
175	`pcl::ModelCoefficients lineCoefficients;`	175	`pcl::ModelCoefficients lineCoefficients;`
176	`lineCoefficients.values.resize(6);`	176	`lineCoefficients.values.resize(6);`
177	`lineCoefficients.values[0] = O[0];`	177	`lineCoefficients.values[0] = O[0];`
178	`lineCoefficients.values[1] = O[1];`	178	`lineCoefficients.values[1] = O[1];`
179	`lineCoefficients.values[2] = O[2];`	179	`lineCoefficients.values[2] = O[2];`
180	`lineCoefficients.values[3] = v[0];`	180	`lineCoefficients.values[3] = v[0];`
181	`lineCoefficients.values[4] = v[1];`	181	`lineCoefficients.values[4] = v[1];`
182	`lineCoefficients.values[5] = v[2];`	182	`lineCoefficients.values[5] = v[2];`
183	`visualizer->removeShape("line");`	183	`visualizer->removeShape("line");`
184	`visualizer->addLine(lineCoefficients);`	184	`visualizer->addLine(lineCoefficients);`
185		185
186		-
-		186	`this->repaint();`
187		187
188	`}`	188	`}`
189		189
190	`SMPointCloudWidget::~SMPointCloudWidget(){`	190	`SMPointCloudWidget::~SMPointCloudWidget(){`
191		191
192	`//delete visualizer;`	192	`//delete visualizer;`
193		193
194	`}`	194	`}`
195		195