點雲雙窗口顯示

http://blog.csdn.net/u013682388/article/details/41084467

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include<pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <vector>
#include <ctime>
#include<pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/console/parse.h>
#include "las_file.h"

//加入自己的數據類型

using namespace std;

//
//void viewOneOff(pcl::visualization::PCLVisualizer& viewer)
//{
//  viewer.setBackgroundColor(1,1,1);
//}

//可視化單個點雲
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    //創建3D窗口並添加點雲
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0.5,0.5,0.5);
    viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem (0.1);
    viewer->initCameraParameters ();
    return (viewer);
}

//可視化點雲顏色特徵
boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    //創建3D窗口並添加點雲
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (1, 1, 1);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    //viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();
    return (viewer);
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> customColourVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    //創建3D窗口並添加點雲
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (1, 1, 1);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ> (cloud, single_color, "sample cloud");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    //viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();
    return (viewer);
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> shapesVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    //創建3D窗口並添加點雲
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (255, 255, 255);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem (0.05);
    viewer->initCameraParameters ();
    //在點雲上添加直線和球體模型

    viewer->addLine<pcl::PointXYZRGB> (cloud->points[0],
        cloud->points[cloud->size() - 1], "line");
    //0.2爲半徑  後面爲顏色
    viewer->addSphere (cloud->points[0], 0.02, 0.5, 0.5, 0.0, "sphere");
    //在其他位置添加基於模型參數的平面及圓錐體
    pcl::ModelCoefficients coeffs;
    //values爲一個矩陣
    coeffs.values.push_back (0.0);
    coeffs.values.push_back (0.0);
    coeffs.values.push_back (0.02);
    coeffs.values.push_back (0.0);
    viewer->addPlane (coeffs, "plane");
    coeffs.values.clear ();
    coeffs.values.push_back (0.3);
    coeffs.values.push_back (0.3);
    coeffs.values.push_back (0.0);
    coeffs.values.push_back (0.0);
    coeffs.values.push_back (1.0);
    coeffs.values.push_back (0.0);
    coeffs.values.push_back (5.0);
    viewer->addCone (coeffs, "cone");

    return (viewer);
}

//顯示法向量
boost::shared_ptr<pcl::visualization::PCLVisualizer> normalsVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud,
    pcl::PointCloud<pcl::Normal>::ConstPtr normals)
{
    //創建3D窗口並添加點雲其包括法線
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (255, 255, 255);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    //法向量可以顯示顏色嗎？
    viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals, 500, 0.005, "normals");
    //viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();
    return (viewer);
}

//重頭戲   用kdtree來搜索半徑，
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewportsVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud,
    pcl::PointCloud<pcl::Normal>::ConstPtr normals1, pcl::PointCloud<pcl::Normal>::ConstPtr normals2)
{
    // 創建3D窗口並添加顯示點雲其包括法線
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->initCameraParameters ();
    int v1(0);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    cout<<v1<<endl;
    viewer->setBackgroundColor (0, 0, 0, v1);
    viewer->addText("Radius: 0.01", 10, 10, "v1 text", v1);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud1", v1);
    int v2(0);
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    cout<<v2<<endl;
    viewer->setBackgroundColor (0.3, 0.3, 0.3, v2);
    viewer->addText("Radius: 0.1", 10, 10, "v2 text", v2);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color(cloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, single_color, "sample cloud2", v2);

    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud1");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud2");
    viewer->addCoordinateSystem (1.0);

    viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals1, 10, 0.05, "normals1", v1);
    viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal> (cloud, normals2, 10, 0.05, "normals2", v2);

    return (viewer);
}

int main()
{
    int i = 0;
    CLasOperator clas;
    clas.readLasFile("dragon.las");
    std::vector<Point3D> &point3d = clas.getPointData();

    //pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    //使用智能指針效果最好
    boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    boost::shared_ptr<pcl::PointCloud<pcl::Normal>> normals(new pcl::PointCloud<pcl::Normal>);
    //pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    cloud->points.resize (point3d.size());
    normals->points.resize(point3d.size());
    for(i = 0;i < point3d.size();i++)
    {
        cloud->points[i].x = point3d[i].x;
        cloud->points[i].y = point3d[i].y;
        cloud->points[i].z = point3d[i].z;
        cloud->points[i].r = 255;
        normals->points[i].normal_x = 1;
        normals->points[i].normal_y = 1;
        normals->points[i].normal_z = 1;
    }

    cout<<cloud->size()<<endl;
    // 0.05爲搜索半徑獲取點雲法線
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;

    ne.setInputCloud (cloud);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
    ne.setSearchMethod (tree);

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>);
    cloud_normals1->points.resize(point3d.size());
    ne.setRadiusSearch (0.005);
    //ne.compute (*cloud_normals1);
    //  0.1爲搜索半徑獲取點雲法線
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
    cloud_normals2->points.resize(point3d.size());
    ne.setRadiusSearch (0.001);
    //ne.compute (*cloud_normals2);

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

    viewer = viewportsVis(cloud,cloud_normals1,cloud_normals2);


    //viewer = shapesVis(cloud);
    //viewer = rgbVis(cloud);
    //viewer = simpleVis(cloud);
    //viewer = customColourVis(cloud);
    //viewer = normalsVis(cloud,normals);


    /*pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
    viewer.showCloud(cloud);*/

    //viewer.runOnVisualizationThreadOnce(viewOneOff);
    //while(!viewer.wasStopped())
    //{

    //}
    while (!viewer->wasStopped ())
    {
       viewer->spinOnce(100);
      // boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}