Subversion Repositories seema-scanner

Rev

Rev 31 | Rev 34 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 31 Rev 33
Line 1... Line 1...
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 
 
Line 34... Line 35...
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 
 
Line 134... Line 136...
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;
Line 155... Line 167...
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