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