PCL可视化对于机载点云显示不出的问题及其解决

原创

Tech沉思录 2023-03-04 00:11:56 博主文章分类：C/C++ ©著作权

文章标签 PCL #include 归一化 i++ 文章分类 MySQL 数据库

©著作权归作者所有：来自51CTO博客作者Tech沉思录的原创作品，请联系作者获取转载授权，否则将追究法律责任

1、问题

在pcl中对于机载的点云，其特点一个是坐标特别大，大的离谱。在pcl的可视化中可能由于坐标距离原点(0,0,0)特别远，所以显示不出来；第二个是每个点之间的间距也特别大，可能显示的时候是只显示出了其中的某一个点。

2、思路

开始的时候没有考虑到点之间的距离大的问题，只是将pcd文件中的每一个坐标点平移最大点的坐标的距离，结果是还是显示不出来，第二次参考了一篇博客将所有点进行归一化，变至(-1,1)这个区间，问题解决！

3、代码

核心代码

1、先计算每个轴上的平均值。在对应的坐标减去平均值的坐标。

2、在x轴上进行缩放，都除以x轴上的长度

middlex = (max_bound.x + min_bound.x) / 2;
    middley = (max_bound.y + min_bound.y) / 2;
    middlez = (max_bound.z + min_bound.z) / 2;
 
    for(int i = 0;i<cloud_back->size();i++)
    {
        cloud_back->points[i].x = cloud_in->points[i].x - middlex;
        cloud_back->points[i].y = cloud_in->points[i].y - middley;
        cloud_back->points[i].z = cloud_in->points[i].z - middlez;
    }
 
    //scale based on x range
    double scalecoe = (max_bound.x - min_bound.x) / 2;
    for (int i = 0; i < cloud_back->size(); i++) {
        cloud_back->points[i].x = cloud_back->points[i].x/ scalecoe;
        cloud_back->points[i].y = cloud_back->points[i].y/ scalecoe;
        cloud_back->points[i].z = cloud_back->points[i].z/ scalecoe;
    }

//主要是定义一个函数找到pcd文件的最大点最小点
//然后在另外一个函数中理由最值点对点云进行归一化
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vector>

using namespace std;

struct Point{
    double x,y,z;
};

vector<Point> findMaxAndMin(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
pcl::PointCloud<pcl::PointXYZ>::Ptr normialize(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, vector<Point> MaxAndMin,string filename);
int main(int argc,char** argv)
{
    if(argc<2){
        cout<<"Format: ./normalized filename.pcd"<<endl;
        return -1;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1],*cloud);
    cout<<"Load " <<argv[1]<<endl;
//    normialize(cloud,findMaxAndMin(cloud));
    vector<Point> p = findMaxAndMin(cloud); //输入点云的最值点
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_n (new pcl::PointCloud<pcl::PointXYZ>); //存储归一化之后的点云
    cloud_n = normialize(cloud,p,argv[1]); //归一化

    cout<<"\nafter normalized:"<<endl;
    vector<Point> p_ = findMaxAndMin(cloud_n); //归一化点云之后的最值点


    //可视化
    pcl::visualization::PCLVisualizer viewer("3D Viewer");
//    int v1(0);
//
//    viewer.createViewPort(0.5,0,1,1,v1);
//    viewer.setBackgroundColor (1, 1, 1);
//    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> point_cloud_color_handler (cloud, 0, 0, 0);
//    viewer.addPointCloud (cloud, point_cloud_color_handler, "cloud",v1);

    int v2(0);
    viewer.createViewPort(0,0,0.5,1,v2);
    viewer.setBackgroundColor(1,200,200);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(cloud_n, 0,0,0);
    viewer.addPointCloud(cloud_n,color2,"cloud_n",v2);
    viewer.addText("cloud_normalized",0,0,"cloud_n",v2);

    viewer.addCoordinateSystem();
    while(!viewer.wasStopped())
    {
        viewer.spinOnce();
    }


    return 0;
}

vector<Point> findMaxAndMin(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    vector<Point> vector_back;
    Point max, min;
    max.x = cloud_in->points[0].x;
    max.y = cloud_in->points[0].y;
    max.z = cloud_in->points[0].z;

    min.x = cloud_in->points[0].x;
    min.y = cloud_in->points[0].y;
    min.z = cloud_in->points[0].z;

    for(int i = 0;i<cloud_in->size();i++)
    {
        if (cloud_in->points[i].x>=max.x){max.x = cloud_in->points[i].x;}
        if (cloud_in->points[i].x<=min.x){min.x = cloud_in->points[i].x;}

        if (cloud_in->points[i].y>=max.y){max.y = cloud_in->points[i].y;}
        if (cloud_in->points[i].y<=min.y){min.y = cloud_in->points[i].y;}

        if (cloud_in->points[i].z>=max.z){max.z = cloud_in->points[i].z;}
        if (cloud_in->points[i].z<=min.z){min.z = cloud_in->points[i].z;}
    }

    vector_back.push_back(max);
    vector_back.push_back(min);

    cout<<"max: "<<max.x<<","<<max.y<<","<<max.z<<endl;
    cout<<"min: "<<min.x<<","<<min.y<<","<<min.z<<endl;

    return vector_back;

}

pcl::PointCloud<pcl::PointXYZ>::Ptr normialize(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, vector<Point> MaxAndMin,string filename)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_back( new pcl::PointCloud<pcl::PointXYZ>);
    cloud_back->width = cloud_in->width;
    cloud_back->height = cloud_in->height;
    cloud_back->is_dense = cloud_in->is_dense;
    cloud_back->resize(cloud_in->size());
    Point max_bound,min_bound;
    max_bound = MaxAndMin[0];
    min_bound = MaxAndMin[1];
    double middlex, middley, middlez;
    middlex = (max_bound.x + min_bound.x) / 2;
    middley = (max_bound.y + min_bound.y) / 2;
    middlez = (max_bound.z + min_bound.z) / 2;

    for(int i = 0;i<cloud_back->size();i++)
    {
        cloud_back->points[i].x = cloud_in->points[i].x - middlex;
        cloud_back->points[i].y = cloud_in->points[i].y - middley;
        cloud_back->points[i].z = cloud_in->points[i].z - middlez;
    }

    //scale based on x range
    double scalecoe = (max_bound.x - min_bound.x) / 2;
    for (int i = 0; i < cloud_back->size(); i++) {
        cloud_back->points[i].x = cloud_back->points[i].x/ scalecoe;
        cloud_back->points[i].y = cloud_back->points[i].y/ scalecoe;
        cloud_back->points[i].z = cloud_back->points[i].z/ scalecoe;
    }
//    pcl::io::savePCDFileASCII("normalized.pcd",*cloud_back);
    ostringstream oss;
    oss<<"normalized_"<<filename;
    pcl::io::savePCDFileASCII(oss.str(),*cloud_back);

    cout<<"Normalized!"<<endl;
    cout<<oss.str()<<" Saved!"<<endl;
    return cloud_back;

}