Subversion Repositories seema-scanner

Rev

Rev 248 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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