Esempio n. 1
0
void Mapper::viewer()
{
    pcl::visualization::CloudViewer viewer("viewer");
    PointCloud::Ptr globalMap (new PointCloud);

    pcl::VoxelGrid<PointT>	voxel;
    voxel.setLeafSize( resolution, resolution, resolution );

    while (shutdownFlag == false)
    {
        static int cntGlobalUpdate = 0;
        if ( poseGraph.keyframes.size() <= this->keyframe_size )
        {
            usleep(1000);
            continue;
        }
        // keyframe is updated
        PointCloud::Ptr	tmp(new PointCloud());
        if (cntGlobalUpdate % 15 == 0)
        {
            // update all frames
            cout<<"redrawing frames"<<endl;
            globalMap->clear();
            for ( int i=0; i<poseGraph.keyframes.size(); i+=2 )
            {
                PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]);
                *globalMap += *cloud;
            }
        }
        else
        {
            for ( int i=poseGraph.keyframes.size()-1; i>=0 && i>poseGraph.keyframes.size()-6; i-- )
            {
                PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]);
                *globalMap += *cloud;
            }
        }

        cntGlobalUpdate ++ ;
        //voxel
        voxel.setInputCloud( globalMap );
        voxel.filter( *tmp );

        keyframe_size = poseGraph.keyframes.size();
        globalMap->swap( *tmp );
        viewer.showCloud( globalMap );

        cout<<"points in global map: "<<globalMap->points.size()<<endl;
    }
}
int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿
    
    ifstream fin("./data/pose.txt");
    if (!fin)
    {
        cerr<<"cannot find pose file"<<endl;
        return 1;
    }
    
    for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "./data/%s/%d.%s" ); //图像文件格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
        
        double data[7] = {0};
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
        }
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }
    
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    
    cout<<"正在将图像转换为点云..."<<endl;
    
    // 定义点云使用的格式:这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud ); 
    for ( int i=0; i<5; i++ )
    {
        PointCloud::Ptr current( new PointCloud );
        cout<<"转换图像中: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                if ( d >= 7000 ) continue; // 深度太大时不稳定,去掉
                Eigen::Vector3d point; 
                point[2] = double(d)/depthScale; 
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;
                
                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                current->points.push_back( p );
            }
        // depth filter and statistical removal 
        PointCloud::Ptr tmp ( new PointCloud );
        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
        statistical_filter.setMeanK(50);
        statistical_filter.setStddevMulThresh(1.0);
        statistical_filter.setInputCloud(current);
        statistical_filter.filter( *tmp );
        (*pointCloud) += *tmp;
    }
    
    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    
    // voxel filter 
    pcl::VoxelGrid<PointT> voxel_filter; 
    voxel_filter.setLeafSize( 0.01, 0.01, 0.01 );       // resolution 
    PointCloud::Ptr tmp ( new PointCloud );
    voxel_filter.setInputCloud( pointCloud );
    voxel_filter.filter( *tmp );
    tmp->swap( *pointCloud );
    
    cout<<"滤波之后,点云共有"<<pointCloud->size()<<"个点."<<endl;
    
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}