Subversion Repositories seema-scanner

Rev

Rev 236 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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