Subversion Repositories seema-scanner

Rev

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

Rev 43 Rev 44
Line 11... Line 11...
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 <pcl/io/pcd_io.h>
 - 
 
17 
#include <pcl/io/ascii_io.h>
 - 
 
18 
#include <pcl/io/ply_io.h>
 - 
 
19 
#include <pcl/io/png_io.h>
 - 
 
20 
#include <pcl/io/vtk_io.h>
 - 
 
21 
#include <vtkPolyDataWriter.h>
 - 
 
22 
#include <pcl/conversions.h>
 - 
 
23 
 
 - 
 
24 
#include <fstream>
 16 
#include <fstream>
25 
 
 17 
 
26 
 
 18 
 
27 
#include <QFileDialog>
 19 
#include <QFileDialog>
28 
#include <QSettings>
 20 
#include <QSettings>
Line 43... Line 35...
43 
    // this leads to a segfault on OS X
 35 
    // this leads to a segfault on OS X
44 
    visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);
 36 
    visualizer->setCameraPosition(0,0,-50,0,0,0,0,-1,0);
45 
#endif
 37 
#endif
46 
 
 38 
 
47 
    // Initialize point cloud color handler
 39 
    // Initialize point cloud color handler
48 
    colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>();
 40 
    colorHandler = new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal>();
49 
 
 41 
 
50 
    updateCalibrationParameters();
 42 
    updateCalibrationParameters();
51 
    //time.start();
 43 
    //time.start();
52 
}
 44 
}
53 
 
 45 
 
54 
void SMPointCloudWidget::addPointCloud(PointCloudPtr _pointCloudPCL, int id){
 46 
void SMPointCloudWidget::addPointCloud(SMPointCloud pointCloud, int id){
55 
 
 47 
 
56 
    pointCloudPCL = _pointCloudPCL;
 48 
    std::string stringId = QString::number(id).toStdString();
57 
 
 49 
 
58 
    // Note: using the color handler makes a copy of the rgb fields
 50 
    // Note: using the color handler makes a copy of the rgb fields
59 
    colorHandler->setInputCloud(_pointCloudPCL);
 51 
    colorHandler->setInputCloud(pointCloud.pointCloud);
- 
 
 52 
 
- 
 
 53 
    visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, stringId);
- 
 
 54 
    visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, stringId);
- 
 
 55 
 
- 
 
 56 
    // transformation matrix in Eigen format
- 
 
 57 
    cv::Mat TransformCV(3, 4, CV_32F);
- 
 
 58 
    cv::Mat(pointCloud.R).copyTo(TransformCV.colRange(0, 3));
- 
 
 59 
    cv::Mat(pointCloud.T).copyTo(TransformCV.col(3));
- 
 
 60 
    Eigen::Affine3f Transform;
- 
 
 61 
    cv::cv2eigen(TransformCV, Transform.matrix());
60 
 
 62 
 
61 
    visualizer->addPointCloud(pointCloudPCL, *colorHandler, QString::number(id).toStdString());
 63 
    visualizer->updatePointCloudPose(stringId, Transform.inverse());
62 
    visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, QString::number(id).toStdString());
 - 
 
63 
 
 64 
 
64 
    this->update();
 65 
    this->update();
65 
    emit newPointCloudDisplayed();
 66 
    emit pointCloudDataChanged();
66 
 
 67 
 
67 
}
 68 
}
68 
 
 69 
 
69 
void SMPointCloudWidget::updatePointCloud(PointCloudPtr _pointCloudPCL, int id){
 70 
void SMPointCloudWidget::updatePointCloud(SMPointCloud pointCloud, int id){
70 
 
 - 
 
71 
    pointCloudPCL = _pointCloudPCL;
 - 
 
72 
 
 71 
 
73 
    // Note: using the color handler makes a copy of the rgb fields
 72 
    // Note: using the color handler makes a copy of the rgb fields
74 
    colorHandler->setInputCloud(_pointCloudPCL);
 73 
    colorHandler->setInputCloud(pointCloud.pointCloud);
75 
 
 74 
 
76 
    if(!visualizer->updatePointCloud(pointCloudPCL, *colorHandler, QString::number(id).toStdString())){
 75 
    if(!visualizer->updatePointCloud(pointCloud.pointCloud, *colorHandler, QString::number(id).toStdString())){
77 
        visualizer->addPointCloud(pointCloudPCL, *colorHandler, QString::number(id).toStdString());
 76 
        visualizer->addPointCloud(pointCloud.pointCloud, *colorHandler, QString::number(id).toStdString());
78 
        visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, QString::number(id).toStdString());
 77 
        visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, QString::number(id).toStdString());
79 
     }
 78 
     }
80 
 
 79 
 
81 
    this->update();
 80 
    this->update();
82 
    emit newPointCloudDisplayed();
 81 
    emit pointCloudDataChanged();
83 
 
 82 
 
84 
}
 83 
}
85 
 
 84 
 
86 
void SMPointCloudWidget::savePointCloud(){
 85 
void SMPointCloudWidget::removePointCloud(int id){
87 
 
 86 
 
88 
    QString selectedFilter;
 - 
 
89 
    QString fileName = QFileDialog::getSaveFileName(this, "Save Point Cloud", QString(), "*.pcd;;*.ply;;*.vtk;;*.png;;*.txt", &selectedFilter);
 - 
 
90 
    QFileInfo info(fileName);
 - 
 
91 
    QString type = info.suffix();
 87 
    std::string stringId = QString::number(id).toStdString();
92 
    if(type == ""){
 - 
 
93 
        fileName.append(selectedFilter.remove(0,1));
 - 
 
94 
        type = selectedFilter.remove(0,1);
 - 
 
95 
    }
 - 
 
96 
 
 88 
 
97 
    if(type == "pcd"){
 - 
 
98 
        pcl::io::savePCDFileASCII(fileName.toStdString(), *pointCloudPCL);
 - 
 
99 
    } else if(type == "ply"){
 - 
 
100 
        //pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);
 - 
 
101 
        pcl::PLYWriter w;
 - 
 
102 
        // Write to ply in binary without camera
 - 
 
103 
        w.write<pcl::PointXYZRGB> (fileName.toStdString(), *pointCloudPCL, true, false);
 - 
 
104 
    } else if(type == "vtk"){
 - 
 
105 
        pcl::PCLPointCloud2 pointCloud2;
 89 
    visualizer->removePointCloud(stringId);
106 
        pcl::toPCLPointCloud2(*pointCloudPCL, pointCloud2);
 - 
 
107 
        pcl::io::saveVTKFile(fileName.toStdString(), pointCloud2);
 - 
 
108 
 
 90 
 
109 
//        vtkPolyData polyData;
 - 
 
110 
//        pcl::io::pointCloudTovtkPolyData(*pointCloudPCL, polyData);
 - 
 
111 
//        vtkPolyDataWriter::Pointer writer = vtkPolyDataWriter::New();
 - 
 
112 
//        writer->SetInput(polyData);
 - 
 
113 
//        writer->SetFileName(fileName.toStdString());
 - 
 
114 
//        writer->Update();
 91 
    this->update();
115 
    } else if(type == "png"){
 92 
    emit pointCloudDataChanged();
116 
        pcl::io::savePNGFile(fileName.toStdString(), *pointCloudPCL, "rgb");
 - 
 
117 
    } else if(type == "txt"){
 - 
 
118 
        std::ofstream s(fileName.toLocal8Bit());
 - 
 
119 
        for(unsigned int r=0; r<pointCloudPCL->height; r++){
 - 
 
120 
            for(unsigned int c=0; c<pointCloudPCL->width; c++){
 - 
 
121 
                pcl::PointXYZRGB p = pointCloudPCL->at(c, r);
 - 
 
122 
                if(p.x == p.x)
 - 
 
123 
                    s << p.x << " " << p.y << " " << p.z << "\r\n";
 - 
 
124 
            }
 - 
 
125 
        }
 - 
 
126 
        std::flush(s);
 - 
 
127 
        s.close();
 - 
 
128 
    }
 - 
 
129 
 
 93 
 
130 
}
 94 
}
131 
 
 95 
 
- 
 
 96 
//void SMPointCloudWidget::savePointCloud(){
- 
 
 97 
 
- 
 
 98 
//    QString selectedFilter;
- 
 
 99 
//    QString fileName = QFileDialog::getSaveFileName(this, "Save Point Cloud", QString(), "*.pcd;;*.ply;;*.vtk;;*.png;;*.txt", &selectedFilter);
- 
 
 100 
//    QFileInfo info(fileName);
- 
 
 101 
//    QString type = info.suffix();
- 
 
 102 
//    if(type == ""){
- 
 
 103 
//        fileName.append(selectedFilter.remove(0,1));
- 
 
 104 
//        type = selectedFilter.remove(0,1);
- 
 
 105 
//    }
- 
 
 106 
 
- 
 
 107 
//    if(type == "pcd"){
- 
 
 108 
//        pcl::io::savePCDFileASCII(fileName.toStdString(), *pointCloudPCL);
- 
 
 109 
//    } else if(type == "ply"){
- 
 
 110 
//        //pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);
- 
 
 111 
//        pcl::PLYWriter w;
- 
 
 112 
//        // Write to ply in binary without camera
- 
 
 113 
//        w.write<pcl::PointXYZRGB> (fileName.toStdString(), *pointCloudPCL, true, false);
- 
 
 114 
//    } else if(type == "vtk"){
- 
 
 115 
//        pcl::PCLPointCloud2 pointCloud2;
- 
 
 116 
//        pcl::toPCLPointCloud2(*pointCloudPCL, pointCloud2);
- 
 
 117 
//        pcl::io::saveVTKFile(fileName.toStdString(), pointCloud2);
- 
 
 118 
 
- 
 
 119 
////        vtkPolyData polyData;
- 
 
 120 
////        pcl::io::pointCloudTovtkPolyData(*pointCloudPCL, polyData);
- 
 
 121 
////        vtkPolyDataWriter::Pointer writer = vtkPolyDataWriter::New();
- 
 
 122 
////        writer->SetInput(polyData);
- 
 
 123 
////        writer->SetFileName(fileName.toStdString());
- 
 
 124 
////        writer->Update();
- 
 
 125 
//    } else if(type == "png"){
- 
 
 126 
//        pcl::io::savePNGFile(fileName.toStdString(), *pointCloudPCL, "rgb");
- 
 
 127 
//    } else if(type == "txt"){
- 
 
 128 
//        std::ofstream s(fileName.toLocal8Bit());
- 
 
 129 
//        for(unsigned int r=0; r<pointCloudPCL->height; r++){
- 
 
 130 
//            for(unsigned int c=0; c<pointCloudPCL->width; c++){
- 
 
 131 
//                pcl::PointXYZRGB p = pointCloudPCL->at(c, r);
- 
 
 132 
//                if(p.x == p.x)
- 
 
 133 
//                    s << p.x << " " << p.y << " " << p.z << "\r\n";
- 
 
 134 
//            }
- 
 
 135 
//        }
- 
 
 136 
//        std::flush(s);
- 
 
 137 
//        s.close();
- 
 
 138 
//    }
- 
 
 139 
 
- 
 
 140 
//}
- 
 
 141 
 
132 
void SMPointCloudWidget::saveScreenShot(){
 142 
void SMPointCloudWidget::saveScreenShot(){
133 
 
 143 
 
134 
    vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();
 144 
    vtkWindowToImageFilter* filter = vtkWindowToImageFilter::New();
135 
    //filter->SetInput(visualizer->getRenderWindow());
 145 
    //filter->SetInput(visualizer->getRenderWindow());
136 
    filter->Modified();
 146 
    filter->Modified();
Line 174... Line 184...
174 
//    cv::cv2eigen(TransformRCV, TransformR.matrix());
 184 
//    cv::cv2eigen(TransformRCV, TransformR.matrix());
175 
 
 185 
 
176 
//    visualizer->addCoordinateSystem(100, TransformR);
 186 
//    visualizer->addCoordinateSystem(100, TransformR);
177 
 
 187 
 
178 
    // rotation axis at (0,0,0) pointing (0,1,0) in rotation stage frame
 188 
    // rotation axis at (0,0,0) pointing (0,1,0) in rotation stage frame
179 
    cv::Vec3f O(0.0, -100.0, 0.0);
 189 
    cv::Vec3f O(0.0, -300.0, 0.0);
180 
    cv::Vec3f v(0.0, 300.0, 0.0);
 190 
    cv::Vec3f v(0.0, 600.0, 0.0);
181 
 
 191 
 
182 
    // determine coefficients in camera 0 frame
 192 
    // determine coefficients in camera 0 frame
183 
    O = calibration.Rr.t()*O + -calibration.Rr.t()*calibration.Tr;
 193 
    O = calibration.Rr.t()*O + -calibration.Rr.t()*calibration.Tr;
184 
    v = calibration.Rr.t()*v + -calibration.Rr.t()*calibration.Tr - O;
 194 
    v = calibration.Rr.t()*v + -calibration.Rr.t()*calibration.Tr - O;
185 
 
 195